Merge pull request #247 from piqnt/shape-cast

feat: add ShapeCast function

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(4, 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));
+  }
+}

example/list.json

@@ -44,6 +44,7 @@
   "Revolute",
   "RopeJoint",
   "SensorTest",
+  "ShapeCast",
   "ShapeEditing",
   "Shuffle",
   "SliderCrank",

src/collision/Distance.ts

@@ -274,6 +274,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 {
@@ -694,3 +703,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;
+}

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