Method SolvePositionConstraints

DynamicsB2JointWheel.go:398–442

(data B2SolverData)

}

func (joint *B2WheelJoint) SolvePositionConstraints(data B2SolverData) bool {
	cA := data.Positions[joint.M_indexA].C
	aA := data.Positions[joint.M_indexA].A
	cB := data.Positions[joint.M_indexB].C
	aB := data.Positions[joint.M_indexB].A

	qA := MakeB2RotFromAngle(aA)
	qB := MakeB2RotFromAngle(aB)

	rA := B2RotVec2Mul(qA, B2Vec2Sub(joint.M_localAnchorA, joint.M_localCenterA))
	rB := B2RotVec2Mul(qB, B2Vec2Sub(joint.M_localAnchorB, joint.M_localCenterB))
	d := B2Vec2Sub(B2Vec2Add(B2Vec2Sub(cB, cA), rB), rA)

	ay := B2RotVec2Mul(qA, joint.M_localYAxisA)

	sAy := B2Vec2Cross(B2Vec2Add(d, rA), ay)
	sBy := B2Vec2Cross(rB, ay)

	C := B2Vec2Dot(d, ay)

	k := joint.M_invMassA + joint.M_invMassB + joint.M_invIA*joint.M_sAy*joint.M_sAy + joint.M_invIB*joint.M_sBy*joint.M_sBy

	impulse := 0.0
	if k != 0.0 {
		impulse = -C / k
	} else {
		impulse = 0.0
	}

	P := B2Vec2MulScalar(impulse, ay)
	LA := impulse * sAy
	LB := impulse * sBy

	cA.OperatorMinusInplace(B2Vec2MulScalar(joint.M_invMassA, P))
	aA -= joint.M_invIA * LA
	cB.OperatorPlusInplace(B2Vec2MulScalar(joint.M_invMassB, P))
	aB += joint.M_invIB * LB

	data.Positions[joint.M_indexA].C = cA
	data.Positions[joint.M_indexA].A = aA
	data.Positions[joint.M_indexB].C = cB
	data.Positions[joint.M_indexB].A = aB

	return math.Abs(C) <= B2_linearSlop
}

func (joint B2WheelJoint) GetAnchorA() B2Vec2 {
	return joint.M_bodyA.GetWorldPoint(joint.M_localAnchorA)