Skip to content

Commit

Permalink
Improvements to soft body skinning constraints (#1054)
Browse files Browse the repository at this point in the history 
- Multithreading skinned constraints
- Fixed bug where the the skinned position would update in the first iteration, causing a large velocity spike
- Fixed bug where the velocity of vertices would increase indefinitely when resting on the back stop
  • Loading branch information
@jrouwe
jrouwe authored Apr 13, 2024
1 parent 1781415 commit 744900a
Show file tree
Hide file tree
Showing 6 changed files with 108 additions and 41 deletions.
2 changes: 1 addition & 1 deletion Jolt/Physics/Body/BodyManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1092,7 +1092,7 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);

if (inDrawSettings.mDrawSoftBodySkinConstraints)
mp->DrawSkinConstraints(inRenderer, com);
mp->DrawSkinConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);

if (inDrawSettings.mDrawSoftBodyLRAConstraints)
mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
Expand Down
94 changes: 59 additions & 35 deletions Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,7 @@ void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdate

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

for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex; b < mSettings->mDihedralBendConstraints.data() + inEndIndex; ++b)
for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b)
{
Vertex &v0 = mVertices[b->mVertex[0]];
Vertex &v1 = mVertices[b->mVertex[1]];
Expand Down Expand Up @@ -347,7 +347,7 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

// Satisfy volume constraints
for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex; v < mSettings->mVolumeConstraints.data() + inEndIndex; ++v)
for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v)
{
Vertex &v1 = mVertices[v->mVertex[0]];
Vertex &v2 = mVertices[v->mVertex[1]];
Expand Down Expand Up @@ -391,57 +391,66 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
}
}

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

JPH_ASSERT(mSkinStateTransform == inContext.mCenterOfMassTransform, "Skinning state is stale, artifacts will show!");
JPH_PROFILE_FUNCTION();

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

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

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

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

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

// Clamp vertex distance to max distance from skinned position
if (max_distance < FLT_MAX)
{
Vec3 delta = vertex.mPosition - skin_state.mPosition;
Vec3 delta = vertex.mPosition - skin_pos;
float delta_len_sq = delta.LengthSq();
float max_distance_sq = Square(max_distance);
if (delta_len_sq > max_distance_sq)
vertex.mPosition = skin_state.mPosition + delta * sqrt(max_distance_sq / delta_len_sq);
vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq);
}
}
else
{
// Kinematic: Just update the vertex position
vertex.mPosition = skin_state.mPosition;
vertex.mPosition = skin_pos;
}
}
}
Expand All @@ -453,7 +462,7 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext
float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

// Satisfy edge constraints
for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex; e < mSettings->mEdgeConstraints.data() + inEndIndex; ++e)
for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e)
{
Vertex &v0 = mVertices[e->mVertex[0]];
Vertex &v1 = mVertices[e->mVertex[1]];
Expand Down Expand Up @@ -482,7 +491,7 @@ void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEnd

// Satisfy LRA constraints
Vertex *vertices = mVertices.data();
for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex; lra < mSettings->mLRAConstraints.data() + inEndIndex; ++lra)
for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra)
{
JPH_ASSERT(lra->mVertex[0] < mVertices.size());
JPH_ASSERT(lra->mVertex[1] < mVertices.size());
Expand Down Expand Up @@ -672,6 +681,11 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
for (Vertex &v : mVertices)
v.mPosition -= delta;

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

// Offset bounds to match new position
mLocalBounds.Translate(-delta);
mLocalPredictedBounds.Translate(-delta);
Expand Down Expand Up @@ -769,7 +783,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex)
{
// Determine start and end
SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0 };
SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 };
const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start;
const SoftBodySharedSettings::UpdateGroup &current = mSettings->mUpdateGroups[inGroupIndex];

Expand All @@ -779,6 +793,9 @@ void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioConte
// Process bend constraints
ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex);

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

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

Expand All @@ -791,7 +808,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstra
uint num_groups = (uint)mSettings->mUpdateGroups.size();
JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!");
--num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel

// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed);
if (next_group < num_groups || (num_groups == 0 && next_group == 0))
Expand Down Expand Up @@ -820,8 +837,6 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstra

ApplyCollisionConstraintsAndUpdateVelocities(ioContext);

ApplySkinConstraints(ioContext);

uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
if (iteration < mNumIterations)
{
Expand Down Expand Up @@ -900,7 +915,9 @@ void SoftBodyMotionProperties::SkinVertices(RMat44Arg inCenterOfMassTransform, c
JPH_ASSERT(w.mInvBindIndex < num_skin_matrices);
pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos);
}
mSkinState[s.mVertex].mPosition = pos;
SkinState &skin_state = mSkinState[s.mVertex];
skin_state.mPreviousPosition = skin_state.mPosition;
skin_state.mPosition = pos;
}

// Calculate the normals
Expand Down Expand Up @@ -935,7 +952,9 @@ void SoftBodyMotionProperties::SkinVertices(RMat44Arg inCenterOfMassTransform, c
for (const Skinned &s : mSettings->mSkinnedConstraints)
{
Vertex &vertex = mVertices[s.mVertex];
vertex.mPosition = mSkinState[s.mVertex].mPosition;
SkinState &skin_state = mSkinState[s.mVertex];
skin_state.mPreviousPosition = skin_state.mPosition;
vertex.mPosition = skin_state.mPosition;
vertex.mVelocity = Vec3::sZero();
}
}
Expand Down Expand Up @@ -1017,7 +1036,7 @@ inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor i

void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
DrawConstraints(inConstraintColor,
DrawConstraints(inConstraintColor,
[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
return inGroup.mEdgeEndIndex;
},
Expand All @@ -1030,7 +1049,7 @@ void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RM

void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
DrawConstraints(inConstraintColor,
DrawConstraints(inConstraintColor,
[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
return inGroup.mDihedralBendEndIndex;
},
Expand All @@ -1054,7 +1073,7 @@ void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RM

void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
DrawConstraints(inConstraintColor,
DrawConstraints(inConstraintColor,
[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
return inGroup.mVolumeEndIndex;
},
Expand All @@ -1074,19 +1093,24 @@ void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer,
Color::sYellow);
}

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

void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
DrawConstraints(inConstraintColor,
DrawConstraints(inConstraintColor,
[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
return inGroup.mLRAEndIndex;
},
Expand Down
5 changes: 3 additions & 2 deletions Jolt/Physics/SoftBody/SoftBodyMotionProperties.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
void DrawEdgeConstraints(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) const;
void DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
void DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
void DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
#endif // JPH_DEBUG_RENDERER
Expand Down Expand Up @@ -191,6 +191,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
// Information about the state of all skinned vertices
struct SkinState
{
Vec3 mPreviousPosition = Vec3::sNaN();
Vec3 mPosition = Vec3::sNaN();
Vec3 mNormal = Vec3::sNaN();
};
Expand All @@ -211,7 +212,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
void ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);

/// Enforce all skin constraints
void ApplySkinConstraints(const SoftBodyUpdateContext &inContext);
void ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);

/// Enforce all edge constraints
void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
Expand Down
42 changes: 39 additions & 3 deletions Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -463,6 +463,9 @@ void SoftBodySharedSettings::CalculateSkinnedConstraintNormals()

void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
{
// Clear any previous results
mUpdateGroups.clear();

// Create a list of connected vertices
struct Connection
{
Expand All @@ -487,7 +490,7 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)

swap(inV1, inV2);
}
};
};
for (const Edge &c : mEdgeConstraints)
add_connection(c.mVertex[0], c.mVertex[1]);
for (const LRA &c : mLRAConstraints)
Expand All @@ -510,6 +513,7 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
add_connection(c.mVertex[1], c.mVertex[3]);
add_connection(c.mVertex[2], c.mVertex[3]);
}
// Skinned constraints only update 1 vertex, so we don't need special logic here

// Maps each of the vertices to a group index
Array<int> group_idx;
Expand Down Expand Up @@ -636,13 +640,14 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
{
uint GetSize() const
{
return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size();
return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size();
}

Array<uint> mEdgeConstraints;
Array<uint> mLRAConstraints;
Array<uint> mDihedralBendConstraints;
Array<uint> mVolumeConstraints;
Array<uint> mSkinnedConstraints;
};
Array<Group> groups;
groups.resize(current_group_idx + 1); // + non parallel group
Expand Down Expand Up @@ -690,6 +695,12 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
else // In different groups -> parallel group
groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data()));
}
for (const Skinned &s : mSkinnedConstraints)
{
int g1 = group_idx[s.mVertex];
JPH_ASSERT(g1 >= 0);
groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data()));
}

// Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete)
QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); });
Expand Down Expand Up @@ -792,6 +803,19 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)

return inLHS < inRHS;
});

// Sort the skinned constraints
QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS)
{
const Skinned &s1 = mSkinnedConstraints[inLHS];
const Skinned &s2 = mSkinnedConstraints[inRHS];

// Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges).
if (s1.mVertex != s2.mVertex)
return s1.mVertex < s2.mVertex;

return inLHS < inRHS;
});
}

// Temporary store constraints as we reorder them
Expand All @@ -815,6 +839,11 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
mVolumeConstraints.reserve(temp_volume.size());
outResults.mVolumeRemap.reserve(temp_volume.size());

Array<Skinned> temp_skinned;
temp_skinned.swap(mSkinnedConstraints);
mSkinnedConstraints.reserve(temp_skinned.size());
outResults.mSkinnedRemap.reserve(temp_skinned.size());

// Finalize update groups
for (const Group &group : groups)
{
Expand Down Expand Up @@ -846,8 +875,15 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
outResults.mVolumeRemap.push_back(idx);
}

// Reorder skinned constraints for this group
for (uint idx : group.mSkinnedConstraints)
{
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() });
mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() });
}

// Free closest kinematic buffer
Expand Down
Loading

0 comments on commit 744900a

Please sign in to comment.