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); } }