func (this *PivotJoint) ApplyImpulse() {
a, b := this.BodyA, this.BodyB
r1, r2 := this.r1, this.r2
// compute relative velocity
vr := relative_velocity2(a, b, r1, r2)
// compute normal impulse
j := mult_k(vect.Sub(this.bias, vr), this.k1, this.k2)
jOld := this.jAcc
this.jAcc = vect.Clamp(vect.Add(this.jAcc, j), this.jMaxLen)
j = vect.Sub(this.jAcc, jOld)
// apply impulse
apply_impulses(a, b, this.r1, this.r2, j)
}
func (this *PivotJoint) PreStep(dt vect.Float) {
a, b := this.BodyA, this.BodyB
this.r1 = transform.RotateVect(this.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
this.r2 = transform.RotateVect(this.Anchor2, transform.Rotation{b.rot.X, b.rot.Y})
// Calculate mass tensor
k_tensor(a, b, this.r1, this.r2, &this.k1, &this.k2)
// compute max impulse
this.jMaxLen = this.MaxForce * dt
// calculate bias velocity
delta := vect.Sub(vect.Add(b.p, this.r2), vect.Add(a.p, this.r1))
this.bias = vect.Clamp(vect.Mult(delta, -bias_coef(this.ErrorBias, dt)/dt), this.MaxBias)
}