public void init(CNode a, CNode b)
{
m1 = a;
m2 = b;
Vector3 forceDirection = m2.GetPos() - m1.GetPos();
mRestLegnth = forceDirection.magnitude;
}
public void ComputeSpringForce()
{
Vector3 forceDirection = m2.GetPos() - m1.GetPos();
Vector3 velocityDirection = m2.GetVel() - m1.GetVel();
float len = forceDirection.magnitude;
len = Rounding(len, 4);
float spforce = (float)((len - mRestLegnth) * Ks);
float damp = (float)((Vector3.Dot(velocityDirection, forceDirection) / len) * Kd);
Vector3 nSPF = ((spforce + damp) * forceDirection) / len;
m1.AddForce(nSPF);
m2.AddForce(-nSPF);
//m1.AddForce(Vector3.zero);
//m2.AddForce(Vector3.zero);
}
