Function B2Distance

CollisionB2Distance.go:566–708

(output *B2DistanceOutput, cache *B2SimplexCache, input *B2DistanceInput)

}

func B2Distance(output *B2DistanceOutput, cache *B2SimplexCache, input *B2DistanceInput) {
	b2_gjkCalls++

	proxyA := &input.ProxyA
	proxyB := &input.ProxyB

	transformA := input.TransformA
	transformB := input.TransformB

	// Initialize the simplex.
	simplex := MakeB2Simplex()
	simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB)

	// Get simplex vertices as an array.
	vertices := &simplex.M_vs
	k_maxIters := 20

	// These store the vertices of the last simplex so that we
	// can check for duplicates and prevent cycling.
	saveA := make([]int, 3)
	saveB := make([]int, 3)
	saveCount := 0

	// Main iteration loop.
	iter := 0
	for iter < k_maxIters {
		// Copy simplex so we can identify duplicates.
		saveCount = simplex.M_count
		for i := 0; i < saveCount; i++ {
			saveA[i] = vertices[i].IndexA
			saveB[i] = vertices[i].IndexB
		}

		switch simplex.M_count {
		case 1:
			break

		case 2:
			simplex.Solve2()
			break

		case 3:
			simplex.Solve3()
			break

		default:
			B2Assert(false)
		}

		// If we have 3 points, then the origin is in the corresponding triangle.
		if simplex.M_count == 3 {
			break
		}

		// Get search direction.
		d := simplex.GetSearchDirection()

		// Ensure the search direction is numerically fit.