From 9a2aa944c5c496d7692864c602a6291f294106e1 Mon Sep 17 00:00:00 2001 From: SR Date: Sun, 9 Feb 2020 21:02:05 +0300 Subject: [PATCH 1/3] B2ShapeCast Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. --- CollisionB2Distance.go | 188 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 188 insertions(+) diff --git a/CollisionB2Distance.go b/CollisionB2Distance.go index fbf4b28..f8e8269 100644 --- a/CollisionB2Distance.go +++ b/CollisionB2Distance.go @@ -1,5 +1,7 @@ package box2d +import "math" + /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// @@ -101,6 +103,52 @@ func NewB2DistanceOutput() *B2DistanceOutput { 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 { @@ -152,6 +200,8 @@ func (p B2DistanceProxy) GetSupportVertex(d B2Vec2) B2Vec2 { // 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: @@ -706,3 +756,141 @@ func B2Distance(output *B2DistanceOutput, cache *B2SimplexCache, input *B2Distan } } } + +// 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. +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 && math.Abs(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++ + } + + // 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 +} From e24fc3be6ba063fd2273c5dd58ac566b2faf79d9 Mon Sep 17 00:00:00 2001 From: SR Date: Wed, 12 Feb 2020 19:09:09 +0300 Subject: [PATCH 2/3] shape cast test --- cpp_compliance_shape_cast_test.go | 79 +++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 cpp_compliance_shape_cast_test.go diff --git a/cpp_compliance_shape_cast_test.go b/cpp_compliance_shape_cast_test.go new file mode 100644 index 0000000..698fc2e --- /dev/null +++ b/cpp_compliance_shape_cast_test.go @@ -0,0 +1,79 @@ +package box2d_test + +import ( + "fmt" + "github.com/ByteArena/box2d" + "github.com/pmezard/go-difflib/difflib" + "testing" +) + +var expectedShapeCast string = "hit = false, iters = 3, lambda = 1, distance = 7.040063920164362" + +func TestCPPComplianceShapeCast(t *testing.T) { + transformA := box2d.MakeB2Transform() + transformA.P = box2d.MakeB2Vec2(0.0, 0.25) + transformA.Q.SetIdentity() + + transformB := box2d.MakeB2Transform() + transformB.SetIdentity() + + input := box2d.MakeB2ShapeCastInput() + + pA := box2d.MakeB2DistanceProxy() + pA.M_vertices = append(pA.M_vertices, box2d.MakeB2Vec2(-0.5, 1.0)) + pA.M_vertices = append(pA.M_vertices, box2d.MakeB2Vec2(0.5, 1.0)) + pA.M_vertices = append(pA.M_vertices, box2d.MakeB2Vec2(0.0, 0.0)) + pA.M_count = 3 + pA.M_radius = box2d.B2_polygonRadius + + pB := box2d.MakeB2DistanceProxy() + pB.M_vertices = append(pB.M_vertices, box2d.MakeB2Vec2(-0.5, -0.5)) + pB.M_vertices = append(pB.M_vertices, box2d.MakeB2Vec2(0.5, -0.5)) + pB.M_vertices = append(pB.M_vertices, box2d.MakeB2Vec2(0.5, 0.5)) + pB.M_vertices = append(pB.M_vertices, box2d.MakeB2Vec2(-0.5, 0.5)) + pB.M_count = 4 + pB.M_radius = box2d.B2_polygonRadius + + input.ProxyA = pA + input.ProxyB = pB + input.TransformA = transformA + input.TransformB = transformB + input.TranslationB.Set(8.0, 0.0) + + output := box2d.MakeB2ShapeCastOutput() + + hit := box2d.B2ShapeCast(&output, &input) + + transformB2 := box2d.MakeB2Transform() + transformB2.Q = transformB.Q + transformB2.P = box2d.B2Vec2Add(transformB.P, box2d.B2Vec2MulScalar(output.Lambda, input.TranslationB)) + + distanceInput := box2d.MakeB2DistanceInput() + distanceInput.ProxyA = pA + distanceInput.ProxyB = pB + distanceInput.TransformA = transformA + distanceInput.TransformB = transformB2 + distanceInput.UseRadii = false + simplexCache := box2d.MakeB2SimplexCache() + simplexCache.Count = 0 + distanceOutput := box2d.MakeB2DistanceOutput() + + box2d.B2Distance(&distanceOutput, &simplexCache, &distanceInput) + + msg := fmt.Sprintf("hit = %v, iters = %v, lambda = %v, distance = %.15f", + hit, output.Iterations, output.Lambda, distanceOutput.Distance) + + fmt.Println(msg) + + if msg != expectedShapeCast { + diff := difflib.UnifiedDiff{ + A: difflib.SplitLines(expectedShapeCast), + B: difflib.SplitLines(msg), + FromFile: "Expected", + ToFile: "Current", + Context: 0, + } + text, _ := difflib.GetUnifiedDiffString(diff) + t.Fatalf("NOT Matching c++ reference. Failure: \n%s", text) + } +} From 49893b851a357c44f8c47f2c4526055333203e2b Mon Sep 17 00:00:00 2001 From: SR Date: Wed, 12 Feb 2020 19:22:08 +0300 Subject: [PATCH 3/3] shape cast test with second set of parameters --- cpp_compliance_shape_cast_test.go | 66 +++++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/cpp_compliance_shape_cast_test.go b/cpp_compliance_shape_cast_test.go index 698fc2e..b9323e0 100644 --- a/cpp_compliance_shape_cast_test.go +++ b/cpp_compliance_shape_cast_test.go @@ -77,3 +77,69 @@ func TestCPPComplianceShapeCast(t *testing.T) { t.Fatalf("NOT Matching c++ reference. Failure: \n%s", text) } } + +var expectedShapeCast2 string = "hit = true, iters = 20, lambda = 0, distance = 0.250000000000000" + +func TestCPPComplianceShapeCast2(t *testing.T) { + transformA := box2d.MakeB2Transform() + transformA.P = box2d.MakeB2Vec2(0.0, 0.25) + transformA.Q.SetIdentity() + + transformB := box2d.MakeB2Transform() + transformB.SetIdentity() + + input := box2d.MakeB2ShapeCastInput() + + pA := box2d.MakeB2DistanceProxy() + pA.M_vertices = append(pA.M_vertices, box2d.MakeB2Vec2(0.0, 0.0)) + pA.M_count = 1 + pA.M_radius = 0.5 + + pB := box2d.MakeB2DistanceProxy() + pB.M_vertices = append(pB.M_vertices, box2d.MakeB2Vec2(0.0, 0.0)) + pB.M_count = 1 + pB.M_radius = 0.5 + + input.ProxyA = pA + input.ProxyB = pB + input.TransformA = transformA + input.TransformB = transformB + input.TranslationB.Set(8.0, 0.0) + + output := box2d.MakeB2ShapeCastOutput() + + hit := box2d.B2ShapeCast(&output, &input) + + transformB2 := box2d.MakeB2Transform() + transformB2.Q = transformB.Q + transformB2.P = box2d.B2Vec2Add(transformB.P, box2d.B2Vec2MulScalar(output.Lambda, input.TranslationB)) + + distanceInput := box2d.MakeB2DistanceInput() + distanceInput.ProxyA = pA + distanceInput.ProxyB = pB + distanceInput.TransformA = transformA + distanceInput.TransformB = transformB2 + distanceInput.UseRadii = false + simplexCache := box2d.MakeB2SimplexCache() + simplexCache.Count = 0 + distanceOutput := box2d.MakeB2DistanceOutput() + + box2d.B2Distance(&distanceOutput, &simplexCache, &distanceInput) + + msg := fmt.Sprintf("hit = %v, iters = %v, lambda = %v, distance = %.15f", + hit, output.Iterations, output.Lambda, distanceOutput.Distance) + + fmt.Println(msg) + + if msg != expectedShapeCast2 { + diff := difflib.UnifiedDiff{ + A: difflib.SplitLines(expectedShapeCast2), + B: difflib.SplitLines(msg), + FromFile: "Expected", + ToFile: "Current", + Context: 0, + } + text, _ := difflib.GetUnifiedDiffString(diff) + t.Fatalf("NOT Matching c++ reference. Failure: \n%s", text) + } +}