func (spring *DampedSpring) PreStep(dt vect.Float) {
a := spring.BodyA
b := spring.BodyB
spring.r1 = transform.RotateVect(spring.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
spring.r2 = transform.RotateVect(spring.Anchor2, transform.Rotation{a.rot.X, a.rot.Y})
delta := vect.Sub(vect.Add(b.p, spring.r2), vect.Add(a.p, spring.r1))
dist := vect.Length(delta)
if dist == 0 {
dist = vect.Float(math.Inf(1))
}
spring.n = vect.Mult(delta, 1.0/dist)
k := k_scalar(a, b, spring.r1, spring.r2, spring.n)
spring.nMass = 1.0 / k
spring.targetVRN = 0.0
spring.vCoef = vect.Float(1.0 - math.Exp(float64(-spring.Damping*dt*k)))
fSpring := spring.SpringForceFunc(spring, dist)
apply_impulses(a, b, spring.r1, spring.r2, vect.Mult(spring.n, fSpring*dt))
}
func (this *PivotJoint) Impulse() vect.Float {
return vect.Length(this.jAcc)
}