public static void AngleConstraint(FVParticle p0, FVParticle p1, FVParticle p2, float angle)
{
float a = (p1 - p0).magnitude;
float b = (p2 - p0).magnitude;
float distance = Mathf.Sqrt(a * a + b * b - 2 * a * b * Mathf.Cos(angle));
if ((p1 - p2).magnitude < distance)
{
DistanceConstraint(p1, p2, distance);
}
}
private void InitParticles()
{
for (int i = 0; i < 20; i++)
{
for (int j = 0; j < 10; j++)
{
FVParticle p = new FVParticle(new Vector3(i - 9.5f, 0, j - 4.5f));
particles.Add(p);
}
}
particles[0].isFree = false;
particles[9].isFree = false;
particles[199].isFree = false;
particles[190].isFree = false;
}
private void InitBoxHolder()
{
for (int i = 0; i < boxLength; i++)
{
FVParticle p = new FVParticle(new Vector3(i, -0.5f, 0.5f));
particles.Add(p);
p = new FVParticle(new Vector3(i, -0.5f, -0.5f));
particles.Add(p);
p = new FVParticle(new Vector3(i, 0.5f, -0.5f));
particles.Add(p);
p = new FVParticle(new Vector3(i, 0.5f, 0.5f));
particles.Add(p);
}
particles[0].isFree = particles[1].isFree = particles[2].isFree = particles[3].isFree = false;
}
public static void DistanceConstraint(FVParticle p1, FVParticle p2, float distance)
{
Vector3 delta = p2.curPos - p1.curPos;
float currentDis = delta.magnitude;
float ErrorFactor = (currentDis - distance) / currentDis;
if (p1.isFree && p2.isFree)
{
p1.curPos += ErrorFactor * 0.5f * delta * stiffness;
p2.curPos -= ErrorFactor * 0.5f * delta * stiffness;
}
else if (p1.isFree)
{
p1.curPos += ErrorFactor * delta * stiffness;
}
else if (p2.isFree)
{
p2.curPos -= ErrorFactor * delta * stiffness;
}
}
