Method SolvePositionConstraints

DynamicsB2ContactSolver.go:736–815

Sequential solver.

()

// Sequential solver.
func (solver *B2ContactSolver) SolvePositionConstraints() bool {

	minSeparation := 0.0

	for i := 0; i < solver.M_count; i++ {
		pc := &solver.M_positionConstraints[i]

		indexA := pc.IndexA
		indexB := pc.IndexB
		localCenterA := pc.LocalCenterA
		mA := pc.InvMassA
		iA := pc.InvIA
		localCenterB := pc.LocalCenterB
		mB := pc.InvMassB
		iB := pc.InvIB
		pointCount := pc.PointCount

		cA := solver.M_positions[indexA].C
		aA := solver.M_positions[indexA].A

		cB := solver.M_positions[indexB].C
		aB := solver.M_positions[indexB].A

		// Solve normal constraints
		for j := 0; j < pointCount; j++ {
			xfA := MakeB2Transform()
			xfB := MakeB2Transform()

			xfA.Q.Set(aA)
			xfB.Q.Set(aB)
			xfA.P = B2Vec2Sub(cA, B2RotVec2Mul(xfA.Q, localCenterA))
			xfB.P = B2Vec2Sub(cB, B2RotVec2Mul(xfB.Q, localCenterB))

			psm := MakeB2PositionSolverManifold()
			psm.Initialize(pc, xfA, xfB, j)
			normal := psm.Normal

			point := psm.Point
			separation := psm.Separation

			rA := B2Vec2Sub(point, cA)
			rB := B2Vec2Sub(point, cB)

			// Track max constraint error.
			minSeparation = math.Min(minSeparation, separation)

			// Prevent large corrections and allow slop.
			C := B2FloatClamp(B2_baumgarte*(separation+B2_linearSlop), -B2_maxLinearCorrection, 0.0)

			// Compute the effective mass.
			rnA := B2Vec2Cross(rA, normal)
			rnB := B2Vec2Cross(rB, normal)
			K := mA + mB + iA*rnA*rnA + iB*rnB*rnB

			// Compute normal impulse
			impulse := 0.0
			if K > 0.0 {
				impulse = -C / K