public int Compare(object x, object y)
{
ConstraintData a = (ConstraintData)x;
ConstraintData b = (ConstraintData)y;
ParticleData pa1 = m_particles[a.ParticleAIndex];
ParticleData pa2 = m_particles[a.ParticleBIndex];
ParticleData pb1 = m_particles[b.ParticleAIndex];
ParticleData pb2 = m_particles[b.ParticleBIndex];
if (pa1.IsKinematic || pa2.IsKinematic)
{
return(-1);
}
if (pb1.IsKinematic || pb2.IsKinematic)
{
return(1);
}
if (Math.Min(pa1.Mass, pa2.Mass) < Math.Min(pb1.Mass, pb2.Mass))
{
return(-1);
}
return(1);
}
private void UpdateConstraints(float dt)
{
for (int i = 0; i < m_constraints.Length; ++i)
{
ConstraintData constraint = m_constraints[i];
bool kinematic1 = m_kinematic[constraint.ParticleAIndex];
bool kinematic2 = m_kinematic[constraint.ParticleBIndex];
//both are kinematic ignore
if (kinematic1 && kinematic2)
{
continue;
}
float x1 = m_x[constraint.ParticleAIndex];
float y1 = m_y[constraint.ParticleAIndex];
float z1 = m_z[constraint.ParticleAIndex];
float x2 = m_x[constraint.ParticleBIndex];
float y2 = m_y[constraint.ParticleBIndex];
float z2 = m_z[constraint.ParticleBIndex];
float dx = x2 - x1;
float dy = y2 - y1;
float dz = z2 - z1;
float deltaLength = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
if (deltaLength <= 0)
{
continue;
}
float invMass1 = kinematic1 ? 0 : m_invmass[constraint.ParticleAIndex];
float invMass2 = kinematic2 ? 0 : m_invmass[constraint.ParticleBIndex];
float diff = (deltaLength - constraint.Length) / (deltaLength * (invMass1 + invMass2));
dx *= diff;
dy *= diff;
dz *= diff;
SetPosition(constraint.ParticleAIndex, x1 + dx * invMass1, y1 + dy * invMass1, z1 + dz * invMass1, false);
SetPosition(constraint.ParticleBIndex, x2 - dx * invMass2, y2 - dy * invMass2, z2 - dz * invMass2, false);
}
}