Skip to content

Commit

Permalink
feat: add ShapeCast function
Browse files Browse the repository at this point in the history
  • Loading branch information
zOadT committed Aug 5, 2023
1 parent 39a983b commit 9b21249
Show file tree
Hide file tree
Showing 4 changed files with 341 additions and 1 deletion.
164 changes: 164 additions & 0 deletions example/ShapeCast.js
Original file line number Diff line number Diff line change
@@ -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));
}
}
1 change: 1 addition & 0 deletions example/list.json
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
"Revolute",
"RopeJoint",
"SensorTest",
"ShapeCast",
"ShapeEditing",
"Shuffle",
"SliderCrank",
Expand Down
175 changes: 175 additions & 0 deletions src/collision/Distance.ts
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -707,3 +716,169 @@ 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.
*/
//
// 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;
}
2 changes: 1 addition & 1 deletion src/collision/TimeOfImpact.ts
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 9b21249

Please sign in to comment.