示例#1
0
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)
}
示例#2
0
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)
}