CollisionB2Distance.go

package box2d

import "math"

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
// B2Distance.h
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

/// A distance proxy is used by the GJK algorithm.
/// It encapsulates any shape.
type B2DistanceProxy struct {
	M_buffer   [2]B2Vec2
	M_vertices []B2Vec2 // is a memory blob using pointer arithmetic in original implementation
	M_count    int
	M_radius   float64
}

func MakeB2DistanceProxy() B2DistanceProxy {
	return B2DistanceProxy{
		M_vertices: make([]B2Vec2, 0),
		M_count:    0,
		M_radius:   0.0,
	}
}

func NewB2DistanceProxy() *B2DistanceProxy {
	res := MakeB2DistanceProxy()
	return &res
}

/// Used to warm start b2Distance.
/// Set count to zero on first call.
type B2SimplexCache struct {
	Metric float64 ///< length or area
	Count  int
	IndexA [3]int ///< vertices on shape A
	IndexB [3]int ///< vertices on shape B
}

func MakeB2SimplexCache() B2SimplexCache {
	return B2SimplexCache{
		Metric: 0,
		Count:  0,
		IndexA: [3]int{}, ///< vertices on shape A
		IndexB: [3]int{}, ///< vertices on shape B
	}
}

func NewB2SimplexCache() *B2SimplexCache {
	res := MakeB2SimplexCache()
	return &res
}

/// Input for b2Distance.
/// You have to option to use the shape radii
/// in the computation. Even
type B2DistanceInput struct {
	ProxyA     B2DistanceProxy
	ProxyB     B2DistanceProxy
	TransformA B2Transform
	TransformB B2Transform
	UseRadii   bool
}

func MakeB2DistanceInput() B2DistanceInput {
	return B2DistanceInput{
		ProxyA:     MakeB2DistanceProxy(),
		ProxyB:     MakeB2DistanceProxy(),
		TransformA: MakeB2Transform(),
		TransformB: MakeB2Transform(),
		UseRadii:   false,
	}
}

func NewB2DistanceInput() *B2DistanceInput {
	res := MakeB2DistanceInput()
	return &res
}

/// Output for b2Distance.
type B2DistanceOutput struct {
	PointA     B2Vec2 ///< closest point on shapeA
	PointB     B2Vec2 ///< closest point on shapeB
	Distance   float64
	Iterations int ///< number of GJK iterations used
}

func MakeB2DistanceOutput() B2DistanceOutput {
	return B2DistanceOutput{
		PointA:     MakeB2Vec2(0, 0),
		PointB:     MakeB2Vec2(0, 0),
		Distance:   0,
		Iterations: 0,
	}
}

func NewB2DistanceOutput() *B2DistanceOutput {
	res := MakeB2DistanceOutput()
	return &res
}

/// Input parameters for b2ShapeCast
type B2ShapeCastInput struct {
	ProxyA       B2DistanceProxy
	ProxyB       B2DistanceProxy
	TransformA   B2Transform
	TransformB   B2Transform
	TranslationB B2Vec2
}

func MakeB2ShapeCastInput() B2ShapeCastInput {
	return B2ShapeCastInput{
		ProxyA:       MakeB2DistanceProxy(),
		ProxyB:       MakeB2DistanceProxy(),
		TransformA:   MakeB2Transform(),
		TransformB:   MakeB2Transform(),
		TranslationB: MakeB2Vec2(0, 0),
	}
}

func NewB2ShapeCastInput() *B2ShapeCastInput {
	res := MakeB2ShapeCastInput()
	return &res
}

/// Output results for b2ShapeCast
type B2ShapeCastOutput struct {
	Point      B2Vec2
	Normal     B2Vec2
	Lambda     float64
	Iterations int
}

func MakeB2ShapeCastOutput() B2ShapeCastOutput {
	return B2ShapeCastOutput{
		Point:      MakeB2Vec2(0, 0),
		Normal:     MakeB2Vec2(0, 0),
		Lambda:     0.0,
		Iterations: 0,
	}
}

func NewB2ShapeCastOutput() *B2ShapeCastOutput {
	res := MakeB2ShapeCastOutput()
	return &res
}

// //////////////////////////////////////////////////////////////////////////

func (p B2DistanceProxy) GetVertexCount() int {
	return p.M_count
}

func (p B2DistanceProxy) GetVertex(index int) B2Vec2 {
	B2Assert(0 <= index && index < p.M_count)
	return p.M_vertices[index]
}

func (p B2DistanceProxy) GetSupport(d B2Vec2) int {
	bestIndex := 0
	bestValue := B2Vec2Dot(p.M_vertices[0], d)
	for i := 1; i < p.M_count; i++ {
		value := B2Vec2Dot(p.M_vertices[i], d)
		if value > bestValue {
			bestIndex = i
			bestValue = value
		}
	}

	return bestIndex
}

func (p B2DistanceProxy) GetSupportVertex(d B2Vec2) B2Vec2 {
	bestIndex := 0
	bestValue := B2Vec2Dot(p.M_vertices[0], d)

	for i := 1; i < p.M_count; i++ {
		value := B2Vec2Dot(p.M_vertices[i], d)
		if value > bestValue {
			bestIndex = i
			bestValue = value
		}
	}

	return p.M_vertices[bestIndex]
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
// B2Distance.cpp
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
var b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters int

/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
func (p *B2DistanceProxy) Set(shape B2ShapeInterface, index int) {
	switch shape.GetType() {
	case B2Shape_Type.E_circle:
		{
			circle := (shape).(*B2CircleShape)
			p.M_vertices = []B2Vec2{circle.M_p}
			p.M_count = 1
			p.M_radius = circle.M_radius
		}
		break

	case B2Shape_Type.E_polygon:
		{
			polygon := shape.(*B2PolygonShape)
			p.M_vertices = polygon.M_vertices[:]
			p.M_count = polygon.M_count
			p.M_radius = polygon.M_radius
		}
		break

	case B2Shape_Type.E_chain:
		{
			chain := shape.(*B2ChainShape)
			B2Assert(0 <= index && index < chain.M_count)

			p.M_buffer[0] = chain.M_vertices[index]
			if index+1 < chain.M_count {
				p.M_buffer[1] = chain.M_vertices[index+1]
			} else {
				p.M_buffer[1] = chain.M_vertices[0]
			}

			p.M_vertices = p.M_buffer[:]
			p.M_count = 2
			p.M_radius = chain.M_radius
		}
		break

	case B2Shape_Type.E_edge:
		{
			edge := shape.(*B2EdgeShape)
			p.M_vertices = []B2Vec2{edge.M_vertex1, edge.M_vertex2}
			p.M_count = 2
			p.M_radius = edge.M_radius
		}
		break

	default:
		B2Assert(false)
	}
}

type B2SimplexVertex struct {
	WA     B2Vec2  // support point in proxyA
	WB     B2Vec2  // support point in proxyB
	W      B2Vec2  // wB - wA
	A      float64 // barycentric coordinate for closest point
	IndexA int     // wA index
	IndexB int     // wB index
}

func MakeB2SimplexVertex() B2SimplexVertex {
	return B2SimplexVertex{
		WA:     MakeB2Vec2(0, 0),
		WB:     MakeB2Vec2(0, 0),
		W:      MakeB2Vec2(0, 0),
		A:      0,
		IndexA: 0,
		IndexB: 0,
	}
}

func NewB2SimplexVertex() *B2SimplexVertex {
	res := MakeB2SimplexVertex()
	return &res
}

type B2Simplex struct {
	//M_v1, M_v2, M_v3 *B2SimplexVertex
	M_vs    [3]B2SimplexVertex
	M_count int
}

func MakeB2Simplex() B2Simplex {
	return B2Simplex{
		M_vs: [3]B2SimplexVertex{
			MakeB2SimplexVertex(),
			MakeB2SimplexVertex(),
			MakeB2SimplexVertex(),
		},
	}
}

func NewB2Simplex() *B2Simplex {
	res := MakeB2Simplex()
	return &res
}

func (simplex *B2Simplex) ReadCache(cache *B2SimplexCache, proxyA *B2DistanceProxy, transformA B2Transform, proxyB *B2DistanceProxy, transformB B2Transform) {
	B2Assert(cache.Count <= 3)

	// Copy data from cache.
	simplex.M_count = cache.Count
	vertices := &simplex.M_vs
	for i := 0; i < simplex.M_count; i++ {
		v := &vertices[i]
		v.IndexA = cache.IndexA[i]
		v.IndexB = cache.IndexB[i]
		wALocal := proxyA.GetVertex(v.IndexA)
		wBLocal := proxyB.GetVertex(v.IndexB)
		v.WA = B2TransformVec2Mul(transformA, wALocal)
		v.WB = B2TransformVec2Mul(transformB, wBLocal)
		v.W = B2Vec2Sub(v.WB, v.WA)
		v.A = 0.0
	}

	// Compute the new simplex metric, if it is substantially different than
	// old metric then flush the simplex.
	if simplex.M_count > 1 {
		metric1 := cache.Metric
		metric2 := simplex.GetMetric()
		if metric2 < 0.5*metric1 || 2.0*metric1 < metric2 || metric2 < B2_epsilon {
			// Reset the simplex.
			simplex.M_count = 0
		}
	}

	// If the cache is empty or invalid ...
	if simplex.M_count == 0 {
		v := &vertices[0]
		v.IndexA = 0
		v.IndexB = 0
		wALocal := proxyA.GetVertex(0)
		wBLocal := proxyB.GetVertex(0)
		v.WA = B2TransformVec2Mul(transformA, wALocal)
		v.WB = B2TransformVec2Mul(transformB, wBLocal)
		v.W = B2Vec2Sub(v.WB, v.WA)
		v.A = 1.0
		simplex.M_count = 1
	}
}

func (simplex B2Simplex) WriteCache(cache *B2SimplexCache) {
	cache.Metric = simplex.GetMetric()
	cache.Count = simplex.M_count
	vertices := &simplex.M_vs
	for i := 0; i < simplex.M_count; i++ {
		cache.IndexA[i] = vertices[i].IndexA
		cache.IndexB[i] = vertices[i].IndexB
	}
}

func (simplex B2Simplex) GetSearchDirection() B2Vec2 {
	switch simplex.M_count {
	case 1:
		return simplex.M_vs[0].W.OperatorNegate()

	case 2:
		{
			e12 := B2Vec2Sub(simplex.M_vs[1].W, simplex.M_vs[0].W)
			sgn := B2Vec2Cross(e12, simplex.M_vs[0].W.OperatorNegate())
			if sgn > 0.0 {
				// Origin is left of e12.
				return B2Vec2CrossScalarVector(1.0, e12)
			} else {
				// Origin is right of e12.
				return B2Vec2CrossVectorScalar(e12, 1.0)
			}
		}

	default:
		B2Assert(false)
		return B2Vec2_zero
	}
}

func (simplex B2Simplex) GetClosestPoint() B2Vec2 {
	switch simplex.M_count {
	case 0:
		B2Assert(false)
		return B2Vec2_zero

	case 1:
		return simplex.M_vs[0].W

	case 2:
		return B2Vec2Add(
			B2Vec2MulScalar(
				simplex.M_vs[0].A,
				simplex.M_vs[0].W,
			),
			B2Vec2MulScalar(
				simplex.M_vs[1].A,
				simplex.M_vs[1].W,
			),
		)

	case 3:
		return B2Vec2_zero

	default:
		B2Assert(false)
		return B2Vec2_zero
	}
}

func (simplex B2Simplex) GetWitnessPoints(pA *B2Vec2, pB *B2Vec2) {
	switch simplex.M_count {
	case 0:
		B2Assert(false)
		break

	case 1:
		*pA = simplex.M_vs[0].WA
		*pB = simplex.M_vs[0].WB
		break

	case 2:
		*pA = B2Vec2Add(
			B2Vec2MulScalar(simplex.M_vs[0].A, simplex.M_vs[0].WA),
			B2Vec2MulScalar(simplex.M_vs[1].A, simplex.M_vs[1].WA),
		)
		*pB = B2Vec2Add(
			B2Vec2MulScalar(simplex.M_vs[0].A, simplex.M_vs[0].WB),
			B2Vec2MulScalar(simplex.M_vs[1].A, simplex.M_vs[1].WB),
		)
		break

	case 3:
		*pA = B2Vec2Add(
			B2Vec2Add(
				B2Vec2MulScalar(simplex.M_vs[0].A, simplex.M_vs[0].WA),
				B2Vec2MulScalar(simplex.M_vs[1].A, simplex.M_vs[1].WA),
			),
			B2Vec2MulScalar(simplex.M_vs[2].A, simplex.M_vs[2].WA),
		)
		*pB = *pA
		break

	default:
		B2Assert(false)
		break
	}
}

func (simplex B2Simplex) GetMetric() float64 {
	switch simplex.M_count {
	case 0:
		B2Assert(false)
		return 0.0

	case 1:
		return 0.0

	case 2:
		return B2Vec2Distance(simplex.M_vs[0].W, simplex.M_vs[1].W)

	case 3:
		return B2Vec2Cross(
			B2Vec2Sub(simplex.M_vs[1].W, simplex.M_vs[0].W),
			B2Vec2Sub(simplex.M_vs[2].W, simplex.M_vs[0].W),
		)

	default:
		B2Assert(false)
		return 0.0
	}
}

////////////////////////////////////////////////////

// Solve a line segment using barycentric coordinates.
func (simplex *B2Simplex) Solve2() {
	w1 := simplex.M_vs[0].W
	w2 := simplex.M_vs[1].W
	e12 := B2Vec2Sub(w2, w1)

	// w1 region
	d12_2 := -B2Vec2Dot(w1, e12)
	if d12_2 <= 0.0 {
		// a2 <= 0, so we clamp it to 0
		simplex.M_vs[0].A = 1.0
		simplex.M_count = 1
		return
	}

	// w2 region
	d12_1 := B2Vec2Dot(w2, e12)
	if d12_1 <= 0.0 {
		// a1 <= 0, so we clamp it to 0
		simplex.M_vs[1].A = 1.0
		simplex.M_count = 1
		simplex.M_vs[0] = simplex.M_vs[1]
		return
	}

	// Must be in e12 region.
	inv_d12 := 1.0 / (d12_1 + d12_2)
	simplex.M_vs[0].A = d12_1 * inv_d12
	simplex.M_vs[1].A = d12_2 * inv_d12
	simplex.M_count = 2
}

// // Possible regions:
// // - points[2]
// // - edge points[0]-points[2]
// // - edge points[1]-points[2]
// // - inside the triangle
func (simplex *B2Simplex) Solve3() {

	w1 := simplex.M_vs[0].W
	w2 := simplex.M_vs[1].W
	w3 := simplex.M_vs[2].W

	// Edge12
	// [1      1     ][a1] = [1]
	// [w1.e12 w2.e12][a2] = [0]
	// a3 = 0
	e12 := B2Vec2Sub(w2, w1)
	w1e12 := B2Vec2Dot(w1, e12)
	w2e12 := B2Vec2Dot(w2, e12)
	d12_1 := w2e12
	d12_2 := -w1e12

	// Edge13
	// [1      1     ][a1] = [1]
	// [w1.e13 w3.e13][a3] = [0]
	// a2 = 0
	e13 := B2Vec2Sub(w3, w1)
	w1e13 := B2Vec2Dot(w1, e13)
	w3e13 := B2Vec2Dot(w3, e13)
	d13_1 := w3e13
	d13_2 := -w1e13

	// Edge23
	// [1      1     ][a2] = [1]
	// [w2.e23 w3.e23][a3] = [0]
	// a1 = 0
	e23 := B2Vec2Sub(w3, w2)
	w2e23 := B2Vec2Dot(w2, e23)
	w3e23 := B2Vec2Dot(w3, e23)
	d23_1 := w3e23
	d23_2 := -w2e23

	// Triangle123
	n123 := B2Vec2Cross(e12, e13)

	d123_1 := n123 * B2Vec2Cross(w2, w3)
	d123_2 := n123 * B2Vec2Cross(w3, w1)
	d123_3 := n123 * B2Vec2Cross(w1, w2)

	// w1 region
	if d12_2 <= 0.0 && d13_2 <= 0.0 {
		simplex.M_vs[0].A = 1.0
		simplex.M_count = 1
		return
	}

	// e12
	if d12_1 > 0.0 && d12_2 > 0.0 && d123_3 <= 0.0 {
		inv_d12 := 1.0 / (d12_1 + d12_2)
		simplex.M_vs[0].A = d12_1 * inv_d12
		simplex.M_vs[1].A = d12_2 * inv_d12
		simplex.M_count = 2
		return
	}

	// e13
	if d13_1 > 0.0 && d13_2 > 0.0 && d123_2 <= 0.0 {
		inv_d13 := 1.0 / (d13_1 + d13_2)
		simplex.M_vs[0].A = d13_1 * inv_d13
		simplex.M_vs[2].A = d13_2 * inv_d13
		simplex.M_count = 2
		simplex.M_vs[1] = simplex.M_vs[2]
		return
	}

	// w2 region
	if d12_1 <= 0.0 && d23_2 <= 0.0 {
		simplex.M_vs[1].A = 1.0
		simplex.M_count = 1
		simplex.M_vs[0] = simplex.M_vs[1]
		return
	}

	// w3 region
	if d13_1 <= 0.0 && d23_1 <= 0.0 {
		simplex.M_vs[2].A = 1.0
		simplex.M_count = 1
		simplex.M_vs[0] = simplex.M_vs[2]
		return
	}

	// e23
	if d23_1 > 0.0 && d23_2 > 0.0 && d123_1 <= 0.0 {
		inv_d23 := 1.0 / (d23_1 + d23_2)
		simplex.M_vs[1].A = d23_1 * inv_d23
		simplex.M_vs[2].A = d23_2 * inv_d23
		simplex.M_count = 2
		simplex.M_vs[0] = simplex.M_vs[2]
		return
	}

	// Must be in triangle123
	inv_d123 := 1.0 / (d123_1 + d123_2 + d123_3)
	simplex.M_vs[0].A = d123_1 * inv_d123
	simplex.M_vs[1].A = d123_2 * inv_d123
	simplex.M_vs[2].A = d123_3 * inv_d123
	simplex.M_count = 3
}

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.
		if d.LengthSquared() < B2_epsilon*B2_epsilon {
			// The origin is probably contained by a line segment
			// or triangle. Thus the shapes are overlapped.

			// We can't return zero here even though there may be overlap.
			// In case the simplex is a point, segment, or triangle it is difficult
			// to determine if the origin is contained in the CSO or very close to it.
			break
		}

		// Compute a tentative new simplex vertex using support points.
		vertex := &vertices[simplex.M_count]
		vertex.IndexA = proxyA.GetSupport(
			B2RotVec2MulT(transformA.Q, d.OperatorNegate()),
		)
		vertex.WA = B2TransformVec2Mul(transformA, proxyA.GetVertex(vertex.IndexA))
		vertex.IndexB = proxyB.GetSupport(B2RotVec2MulT(transformB.Q, d))
		vertex.WB = B2TransformVec2Mul(transformB, proxyB.GetVertex(vertex.IndexB))
		vertex.W = B2Vec2Sub(vertex.WB, vertex.WA)

		// Iteration count is equated to the number of support point calls.
		iter++
		b2_gjkIters++

		// Check for duplicate support points. This is the main termination criteria.
		duplicate := false
		for i := 0; i < saveCount; i++ {
			if vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i] {
				duplicate = true
				break
			}
		}

		// If we found a duplicate support point we must exit to avoid cycling.
		if duplicate {
			break
		}

		// New vertex is ok and needed.
		simplex.M_count++
	}

	if iter > b2_gjkMaxIters {
		b2_gjkMaxIters = iter
	}

	// Prepare output.
	simplex.GetWitnessPoints(&output.PointA, &output.PointB)
	output.Distance = B2Vec2Distance(output.PointA, output.PointB)
	output.Iterations = iter

	// // Cache the simplex.
	simplex.WriteCache(cache)

	// // Apply radii if requested.
	if input.UseRadii {
		rA := proxyA.M_radius
		rB := proxyB.M_radius

		if output.Distance > rA+rB && output.Distance > B2_epsilon {
			// Shapes are still no overlapped.
			// Move the witness points to the outer surface.
			output.Distance -= rA + rB
			normal := B2Vec2Sub(output.PointB, output.PointA)
			normal.Normalize()
			output.PointA.OperatorPlusInplace(
				B2Vec2MulScalar(rA, normal),
			)
			output.PointB.OperatorMinusInplace(
				B2Vec2MulScalar(rB, normal),
			)
		} else {
			// Shapes are overlapped when radii are considered.
			// Move the witness points to the middle.
			p := B2Vec2MulScalar(
				0.5,
				B2Vec2Add(output.PointA, output.PointB),
			)
			output.PointA = p
			output.PointB = p
			output.Distance = 0.0
		}
	}
}

// GJK-raycast
// Algorithm by Gino van den Bergen.
// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
//
// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
/// @returns true if hit, false if there is no hit or an initial overlap
func B2ShapeCast(output *B2ShapeCastOutput, input *B2ShapeCastInput) bool {
	output.Iterations = 0
	output.Lambda = 1.0
	output.Normal.SetZero()
	output.Point.SetZero()

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

	radiusA := math.Max(proxyA.M_radius, B2_polygonRadius)
	radiusB := math.Max(proxyB.M_radius, B2_polygonRadius)
	radius := radiusA + radiusB

	xfA := input.TransformA
	xfB := input.TransformB

	r := input.TranslationB
	n := MakeB2Vec2(0, 0)
	var lambda float64 = 0.0

	// Initial simplex
	simplex := MakeB2Simplex()
	simplex.M_count = 0

	// Get simplex vertices as an array.
	vertices := &simplex.M_vs
	//b2SimplexVertex* vertices = &simplex.m_v1;

	// Get support point in -r direction
	indexA := proxyA.GetSupport(B2RotVec2MulT(xfA.Q, r.OperatorNegate()))
	wA := B2TransformVec2Mul(xfA, proxyA.GetVertex(indexA))
	indexB := proxyB.GetSupport(B2RotVec2MulT(xfB.Q, r))
	wB := B2TransformVec2Mul(xfB, proxyB.GetVertex(indexB))
	v := B2Vec2Sub(wA, wB)

	// Sigma is the target distance between polygons
	sigma := math.Max(B2_polygonRadius, radius-B2_polygonRadius)
	var tolerance float64 = 0.5 * B2_linearSlop

	// Main iteration loop.
	k_maxIters := 20
	iter := 0
	for iter < k_maxIters && v.Length()-sigma > tolerance {
		B2Assert(simplex.M_count < 3)

		output.Iterations += 1

		// Support in direction -v (A - B)
		indexA = proxyA.GetSupport(B2RotVec2MulT(xfA.Q, v.OperatorNegate()))
		wA = B2TransformVec2Mul(xfA, proxyA.GetVertex(indexA))
		indexB = proxyB.GetSupport(B2RotVec2MulT(xfB.Q, v))
		wB = B2TransformVec2Mul(xfB, proxyB.GetVertex(indexB))
		p := B2Vec2Sub(wA, wB)

		// -v is a normal at p
		v.Normalize()

		// Intersect ray with plane
		vp := B2Vec2Dot(v, p)
		vr := B2Vec2Dot(v, r)
		if vp-sigma > lambda*vr {
			if vr <= 0.0 {
				return false
			}

			lambda = (vp - sigma) / vr
			if lambda > 1.0 {
				return false
			}

			n = v.OperatorNegate()
			simplex.M_count = 0
		}

		// Reverse simplex since it works with B - A.
		// Shift by lambda * r because we want the closest point to the current clip point.
		// Note that the support point p is not shifted because we want the plane equation
		// to be formed in unshifted space.
		vertex := &vertices[simplex.M_count]
		vertex.IndexA = indexB
		vertex.WA = B2Vec2Add(wB, B2Vec2MulScalar(lambda, r))
		vertex.IndexB = indexA
		vertex.WB = wA
		vertex.W = B2Vec2Sub(vertex.WB, vertex.WA)
		vertex.A = 1.0
		simplex.M_count += 1

		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 {
			// Overlap
			return false
		}

		// Get search direction.
		v = simplex.GetClosestPoint()

		// Iteration count is equated to the number of support point calls.
		iter++
	}

	if iter == 0 {
		// Initial overlap
		return false
	}

	// Prepare output.
	pointA := MakeB2Vec2(0, 0)
	pointB := MakeB2Vec2(0, 0)
	simplex.GetWitnessPoints(&pointB, &pointA)

	if v.LengthSquared() > 0.0 {
		n = v.OperatorNegate()
		n.Normalize()
	}

	output.Point = B2Vec2Add(pointA, B2Vec2MulScalar(radiusA, n))
	output.Normal = n
	output.Lambda = lambda
	output.Iterations = iter
	return true
}