From bbcdc54da4b25af8031c43aea9ba5707a6165623 Mon Sep 17 00:00:00 2001 From: Oliver Zell Date: Fri, 4 Aug 2023 21:52:51 +0200 Subject: [PATCH 1/2] feat: add ShapeCast function --- example/ShapeCast.js | 164 +++++++++++++++++++++++++++++++ example/list.json | 1 + src/collision/Distance.ts | 176 ++++++++++++++++++++++++++++++++++ src/collision/TimeOfImpact.ts | 2 +- 4 files changed, 342 insertions(+), 1 deletion(-) create mode 100644 example/ShapeCast.js diff --git a/example/ShapeCast.js b/example/ShapeCast.js new file mode 100644 index 00000000..b5169a4a --- /dev/null +++ b/example/ShapeCast.js @@ -0,0 +1,164 @@ +/* + * MIT License + * Copyright (c) 2019 Erin Catto + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const { Vec2, Transform, Math, World, Settings, ShapeCastInput, ShapeCastOutput, ShapeCast, DistanceInput, DistanceOutput, Distance, SimplexCache } = planck; + +var world = new World(); + +const testbed = planck.testbed(); +testbed.width = 30 +testbed.height = 30 +testbed.start(world); + +const vAs = new Array(3).fill().map(() => Vec2.zero()); +let countA; +let radiusA; + +const vBs = new Array(Settings.maxPolygonVertices).fill().map(() => Vec2.zero()); +let countB; +let radiusB; + +let transformA; +let transformB; +let translationB; + +if (true) { + vAs[0].set(-0.5, 1.0); + vAs[1].set(0.5, 1.0); + vAs[2].set(0.0, 0.0); + countA = 3; + radiusA = Settings.polygonRadius; + + vBs[0].set(-0.5, -0.5); + vBs[1].set(0.5, -0.5); + vBs[2].set(0.5, 0.5); + vBs[3].set(-0.5, 0.5); + countB = 4; + radiusB = Settings.polygonRadius; + + transformA = new Transform(new Vec2(0, 0.25)); + transformB = new Transform(new Vec2(-4, 0)); + translationB = new Vec2(8.0, 0.0); +} else if (true) { + vAs[0].set(0.0, 0.0); + countA = 1; + radiusA = 0.5; + + vBs[0].set(0.0, 0.0); + countB = 1; + radiusB = 0.5; + + transformA = new Transform(new Vec2(0, 0.25)); + transformB = new Transform(new Vec2(-4, 0)); + translationB = new Vec2(8.0, 0.0); +} else { + vAs[0].set(0.0, 0.0); + vAs[1].set(2.0, 0.0); + countA = 2; + radiusA = Settings.polygonRadius; + + vBs[0].set(0.0, 0.0); + countB = 1; + radiusB = 0.25; + + // Initial overlap + transformA = new Transform(new Vec2(0, 0)); + transformB = new Transform(new Vec2(-0.244360745, 0.05999358)); + transformB.q.setIdentity(); + translationB = new Vec2(0.0, 0.0399999991); +} + +testbed.step = function() { + const transformB = Transform.identity(); + + const input = new ShapeCastInput(); + input.proxyA.setVertices(vAs, countA, radiusA); + input.proxyB.setVertices(vBs, countB, radiusB); + input.transformA = transformA; + input.transformB = transformB; + input.translationB = translationB; + + const output = new ShapeCastOutput(); + + const hit = ShapeCast(output, input); + + const transformB2 = new Transform( + Vec2.combine(1, transformB.p, output.lambda, input.translationB), + transformB.q.getAngle() + ); + + const distanceInput = new DistanceInput(); + distanceInput.proxyA.setVertices(vAs, countA, radiusA); + distanceInput.proxyB.setVertices(vBs, countB, radiusB); + distanceInput.transformA = transformA; + distanceInput.transformB = transformB2; + distanceInput.useRadii = false; + const simplexCache = new SimplexCache(); + simplexCache.count = 0; + const distanceOutput = new DistanceOutput(); + + Distance(distanceOutput, simplexCache, distanceInput); + + testbed.status({ + hit, + iters: output.iterations, + lambda: output.lambda, + distance: distanceOutput.distance, + }); + + const vertices = new Array(Settings.maxPolygonVertices); + + for (let i = 0; i < countA; ++i) { + vertices[i] = Transform.mul(transformA, vAs[i]); + } + if (countA == 1) { + testbed.drawCircle(vertices[0], radiusA, testbed.color(0.9, 0.9, 0.9)); + } else { + testbed.drawPolygon(vertices.slice(0, countA), testbed.color(0.9, 0.9, 0.9)); + } + + for (let i = 0; i < countB; ++i) { + vertices[i] = Transform.mul(transformB, vBs[i]); + } + if (countB == 1) { + testbed.drawCircle(vertices[0], radiusB, testbed.color(0.5, 0.9, 0.5)); + } else { + testbed.drawPolygon(vertices.slice(0, countB), testbed.color(0.5, 0.9, 0.5)); + } + + for (let i = 0; i < countB; ++i) { + vertices[i] = Transform.mul(transformB2, vBs[i]); + } + if (countB == 1) { + testbed.drawCircle(vertices[0], radiusB, testbed.color(0.5, 0.7, 0.9)); + } else { + testbed.drawPolygon(vertices.slice(0, countB), testbed.color(0.5, 0.7, 0.9)); + } + + if (hit) { + const p1 = output.point; + testbed.drawPoint(p1, 10.0, testbed.color(0.9, 0.3, 0.3)); + const p2 = Vec2.add(p1, output.normal); + testbed.drawSegment(p1, p2, testbed.color(0.9, 0.3, 0.3)); + } +} diff --git a/example/list.json b/example/list.json index 2d61e550..83b5a93d 100644 --- a/example/list.json +++ b/example/list.json @@ -44,6 +44,7 @@ "Revolute", "RopeJoint", "SensorTest", + "ShapeCast", "ShapeEditing", "Shuffle", "SliderCrank", diff --git a/src/collision/Distance.ts b/src/collision/Distance.ts index ffbf32a3..e2fbceef 100644 --- a/src/collision/Distance.ts +++ b/src/collision/Distance.ts @@ -287,6 +287,15 @@ export class DistanceProxy { _ASSERT && console.assert(typeof shape.computeDistanceProxy === 'function'); shape.computeDistanceProxy(this, index); } + /** + * Initialize the proxy using a vertex cloud and radius. The vertices + * must remain in scope while the proxy is in use. + */ + setVertices(vertices: Vec2[], count: number, radius: number) { + this.m_vertices = vertices; + this.m_count = count; + this.m_radius = radius; + } } class SimplexVertex { @@ -707,3 +716,170 @@ Distance.Input = DistanceInput; Distance.Output = DistanceOutput; Distance.Proxy = DistanceProxy; Distance.Cache = SimplexCache; + +/** + * Input parameters for ShapeCast + */ +export class ShapeCastInput { + proxyA: DistanceProxy = new DistanceProxy(); + proxyB: DistanceProxy = new DistanceProxy(); + transformA: Transform | null = null; + transformB: Transform | null = null; + translationB: Vec2 = Vec2.zero(); +} + +/** + * Output results for b2ShapeCast + */ +export class ShapeCastOutput { + point: Vec2 = Vec2.zero(); + normal: Vec2 = Vec2.zero(); + lambda: number; + iterations: number; +} + +/** + * 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 + */ +// +// GJK-raycast +// Algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 +export const ShapeCast = function(output: ShapeCastOutput, input: ShapeCastInput): boolean { + output.iterations = 0; + output.lambda = 1.0; + output.normal.setZero(); + output.point.setZero(); + + const proxyA = input.proxyA; + const proxyB = input.proxyB; + + const radiusA = Math.max(proxyA.m_radius, Settings.polygonRadius); + const radiusB = Math.max(proxyB.m_radius, Settings.polygonRadius); + const radius = radiusA + radiusB; + + const xfA = input.transformA; + const xfB = input.transformB; + + const r = input.translationB; + const n = Vec2.zero(); + let lambda = 0.0; + + // Initial simplex + const simplex = new Simplex(); + simplex.m_count = 0; + + // Get simplex vertices as an array. + const vertices = simplex.m_v; + + // Get support point in -r direction + let indexA = proxyA.getSupport(Rot.mulTVec2(xfA.q, Vec2.neg(r))); + let wA = Transform.mulVec2(xfA, proxyA.getVertex(indexA)); + let indexB = proxyB.getSupport(Rot.mulTVec2(xfB.q, r)); + let wB = Transform.mulVec2(xfB, proxyB.getVertex(indexB)); + let v = Vec2.sub(wA, wB); + + // Sigma is the target distance between polygons + const sigma = Math.max(Settings.polygonRadius, radius - Settings.polygonRadius); + const tolerance = 0.5 * Settings.linearSlop; + + // Main iteration loop. + const k_maxIters = 20; + let iter = 0; + while (iter < k_maxIters && v.length() - sigma > tolerance) { + _ASSERT && console.assert(simplex.m_count < 3); + + output.iterations += 1; + + // Support in direction -v (A - B) + indexA = proxyA.getSupport(Rot.mulTVec2(xfA.q, Vec2.neg(v))); + wA = Transform.mulVec2(xfA, proxyA.getVertex(indexA)); + indexB = proxyB.getSupport(Rot.mulTVec2(xfB.q, v)); + wB = Transform.mulVec2(xfB, proxyB.getVertex(indexB)); + const p = Vec2.sub(wA, wB); + + // -v is a normal at p + v.normalize(); + + // Intersect ray with plane + const vp = Vec2.dot(v, p); + const vr = Vec2.dot(v, r); + if (vp - sigma > lambda * vr) { + if (vr <= 0.0) { + return false; + } + + lambda = (vp - sigma) / vr; + if (lambda > 1.0) { + return false; + } + + n.setMul(-1, v); + 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. + const vertex = vertices[simplex.m_count]; + vertex.indexA = indexB; + vertex.wA = Vec2.combine(1, wB, lambda, r); + vertex.indexB = indexA; + vertex.wB = wA; + vertex.w = Vec2.sub(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: + _ASSERT && console.assert(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. + const pointA = Vec2.zero(); + const pointB = Vec2.zero(); + simplex.getWitnessPoints(pointB, pointA); + + if (v.lengthSquared() > 0.0) { + n.setMul(-1, v); + n.normalize(); + } + + output.point = Vec2.combine(1, pointA, radiusA, n); + output.normal = n; + output.lambda = lambda; + output.iterations = iter; + return true; +} diff --git a/src/collision/TimeOfImpact.ts b/src/collision/TimeOfImpact.ts index 859b328e..6d1bed42 100644 --- a/src/collision/TimeOfImpact.ts +++ b/src/collision/TimeOfImpact.ts @@ -75,7 +75,7 @@ stats.toiMaxRootIters = 0; /** * Compute the upper bound on time before two shapes penetrate. Time is * represented as a fraction between [0,tMax]. This uses a swept separating axis - * and may miss some intermediate, non-tunneling collision. If you change the + * and may miss some intermediate, non-tunneling collisions. If you change the * time interval, you should call this function again. * * Note: use Distance to compute the contact point and normal at the time of From 05352bd7b61c2efb68f2b81130eb8ee319067b78 Mon Sep 17 00:00:00 2001 From: Oliver Zell Date: Sat, 5 Aug 2023 10:14:42 +0200 Subject: [PATCH 2/2] docs: make ShapeCast example more interesting --- example/ShapeCast.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/ShapeCast.js b/example/ShapeCast.js index b5169a4a..11f73cdb 100644 --- a/example/ShapeCast.js +++ b/example/ShapeCast.js @@ -56,7 +56,7 @@ if (true) { countB = 4; radiusB = Settings.polygonRadius; - transformA = new Transform(new Vec2(0, 0.25)); + transformA = new Transform(new Vec2(4, 0.25)); transformB = new Transform(new Vec2(-4, 0)); translationB = new Vec2(8.0, 0.0); } else if (true) {