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