Method SolvePositionConstraints

DynamicsB2JointRevolute.go:375–455

(data B2SolverData)

}

func (joint *B2RevoluteJoint) 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)

	angularError := 0.0
	positionError := 0.0

	fixedRotation := (joint.M_invIA+joint.M_invIB == 0.0)

	// Solve angular limit constraint.
	if joint.M_enableLimit && joint.M_limitState != B2LimitState.E_inactiveLimit && fixedRotation == false {
		angle := aB - aA - joint.M_referenceAngle
		limitImpulse := 0.0

		if joint.M_limitState == B2LimitState.E_equalLimits {
			// Prevent large angular corrections
			C := B2FloatClamp(angle-joint.M_lowerAngle, -B2_maxAngularCorrection, B2_maxAngularCorrection)
			limitImpulse = -joint.M_motorMass * C
			angularError = math.Abs(C)
		} else if joint.M_limitState == B2LimitState.E_atLowerLimit {
			C := angle - joint.M_lowerAngle
			angularError = -C

			// Prevent large angular corrections and allow some slop.
			C = B2FloatClamp(C+B2_angularSlop, -B2_maxAngularCorrection, 0.0)
			limitImpulse = -joint.M_motorMass * C
		} else if joint.M_limitState == B2LimitState.E_atUpperLimit {
			C := angle - joint.M_upperAngle
			angularError = C

			// Prevent large angular corrections and allow some slop.
			C = B2FloatClamp(C-B2_angularSlop, 0.0, B2_maxAngularCorrection)
			limitImpulse = -joint.M_motorMass * C
		}

		aA -= joint.M_invIA * limitImpulse
		aB += joint.M_invIB * limitImpulse
	}

	// Solve point-to-point constraint.
	{
		qA.Set(aA)
		qB.Set(aB)
		rA := B2RotVec2Mul(qA, B2Vec2Sub(joint.M_localAnchorA, joint.M_localCenterA))
		rB := B2RotVec2Mul(qB, B2Vec2Sub(joint.M_localAnchorB, joint.M_localCenterB))

		C := B2Vec2Sub(B2Vec2Sub(B2Vec2Add(cB, rB), cA), rA)
		positionError = C.Length()

		mA := joint.M_invMassA
		mB := joint.M_invMassB
		iA := joint.M_invIA
		iB := joint.M_invIB