/**
* Stiffness constraint
**/
void SolveColission()
{
for (int SegIdx = 0; SegIdx < segments + 1; SegIdx++)
{
CableParticle particleA = points[SegIdx];
if (particleA.IsFree() && particleA.Position.y < (cableWidth))
{
particleA.Position += (cableWidth - particleA.Position.y) * Vector3.up - particleA.Velocity * friction;
}
}
}
void SolveDistanceConstraint(CableParticle particleA, CableParticle particleB, float segmentLength)
{
Vector3 delta = particleB.Position - particleA.Position;
float currentDistance = delta.magnitude;
float errorFactor = (currentDistance - segmentLength) / currentDistance;
if (particleA.IsFree() && particleB.IsFree())
{
particleA.Position += errorFactor * 0.5f * delta;
particleB.Position -= errorFactor * 0.5f * delta;
}
else if (particleA.IsFree())
{
particleA.Position += errorFactor * delta;
}
else if (particleB.IsFree())
{
particleB.Position -= errorFactor * delta;
}
}
/**
* Distance Constraint
*
* This is the main constrains that keeps the cable particles "tied" together.
*/
void SolveDistanceConstraint(CableParticle particleA, CableParticle particleB, float segmentLength)
{
// Find current vector between particles
Vector3 delta = particleB.Position - particleA.Position;
//
float currentDistance = delta.magnitude;
float errorFactor = (currentDistance - segmentLength) / currentDistance;
// Only move free particles to satisfy constraints
if (particleA.IsFree() && particleB.IsFree())
{
particleA.Position += errorFactor * 0.5f * delta;
particleB.Position -= errorFactor * 0.5f * delta;
}
else if (particleA.IsFree())
{
particleA.Position += errorFactor * delta;
}
else if (particleB.IsFree())
{
particleB.Position -= errorFactor * delta;
}
}