class GJKResult {
constructor() {
this.collide = false;
this.simplex = new Simplex();
}
}
const GJK_MAX_ITERATION = 20;
// params b1, b2; : RigidBody
// returns : GJKResult object
function GJK(b1, b2) {
const origin = new Vec2(0, 0);
let simplex = new Simplex();
let dir = new Vec2(1, 0); // Random initial direction
let result /* GJKResult */ = { collide: false, simplex: simplex };
let supportPoint = csoSupport(b1, b2, dir);
simplex.addVertex(supportPoint);
for (let k = 0; k < GJK_MAX_ITERATION; k++) {
let closest = simplex.getClosest(origin);
//console.log("iteration#",k, "closest=", closest);
if (Util.squared_distance(closest.result, origin) < Epsilon.E) {
result.collide = true;
break;
}
if (simplex.count != 1) {
// Rebuild the simplex with vertices that are used(involved) to calculate closest distance
simplex.shrink(closest.contributors);
}
dir = Vec2.Subtract(origin, closest.result);
supportPoint = csoSupport(b1, b2, dir);
// If the new support point is not further along the search direction than the closest point,
// two objects are not colliding so you can early return here.
if (dir.length > Vec2.Normalize(dir).dot(Vec2.Subtract(supportPoint, closest.result))) {
result.collide = false;
break;
}
if (simplex.containsVertex(supportPoint)) {
result.collide = false;
break;
}
else
{
simplex.addVertex(supportPoint);
}
}
result.simplex = simplex;
return result;
}
The most complex part of the code
class ClosestResult {
constructor() {
this.result = new Vec2();
this.contributors = []; // Vertex indices that contributed to calculating the closest point
}
}
class Simplex {
constructor() {
this.vertices = [];
}
get count() {
return this.vertices.length;
}
clear() {
this.vertices = [];
}
// Returns the closest point to the input q
getClosest(q /*: Vec2*/ ) /*: ClosestResult*/ {
switch (this.count) {
// 0-Simplex: Point
case 1: {
return { result: this.vertices[0], contributors: [0] };
}
// 1-Simplex: Line segment
case 2: {
const a = this.vertices[0];
const b = this.vertices[1];
const w = getUV(a, b, q);
if (w.v <= 0)
return { result: a, contributors: [0] };
else if (w.v >= 1)
return { result: b, contributors: [1] };
else
return { result: lerpVector(a, b, w), contributors: [0, 1] };
}
// 2-Simplex: Triangle
case 3: {
const a = this.vertices[0];
const b = this.vertices[1];
const c = this.vertices[2];
const wab = getUV(a, b, q);
const wbc = getUV(b, c, q);
const wca = getUV(c, a, q);
if (wca.u <= 0 && wab.v <= 0) // A area
return { result: a, contributors: [0] };
else if (wab.u <= 0 && wbc.v <= 0) // B area
return { result: b, contributors: [1] };
else if (wbc.u <= 0 && wca.v <= 0) // C area
return { result: c, contributors: [2] };
const area = Vec2.Subtract(b,a).cross(Vec2.Subtract(c,a));
// If area == 0, 3 vertices are in collinear position, which means all aligned in a line
const u = Vec2.Subtract(b, q).cross(Vec2.Subtract(c,q));
const v = Vec2.Subtract(c, q).cross(Vec2.Subtract(a,q));
const w = Vec2.Subtract(a, q).cross(Vec2.Subtract(b,q));
if (wab.u > 0 && wab.v > 0 && w * area <= 0) // On the AB edge
{
return {
result: lerpVector(a, b, wab),
contributors: area != 0 ? [0, 1] : [0, 1, 2]
};
}
else if (wbc.u > 0 && wbc.v > 0 && u * area <= 0) // On the BC edge
{
return {
result: lerpVector(b, c, wbc),
contributors: area != 0 ? [1, 2] : [0, 1, 2]
};
}
else if (wca.u > 0 && wca.u > 0 && v * area <= 0) // On the CA edge
{
return {
result: lerpVector(c, a, wca),
contributors: area != 0 ? [2, 0] : [0, 1, 2]
};
}
else // Inside the triangle
{
return { result: q, contributors: [] };
}
}
default:
throw "Error: Simplex constains vertices more than 3";
}
}
addVertex(vertex) {
if (this.count >= 3) throw "2-simplex can have verticies less than 4";
this.vertices.push(vertex);
}
// Return true if this simplex contains input vertex
containsVertex(vertex) {
for (let i = 0; i < this.count; i++)
{
if (vertex.equals(this.vertices[i]))
return true;
}
return false;
}
shrink(indices) {
let res = [];
for (let i = 0; i < indices.length; i++) {
res.push(this.vertices[indices[i]]);
}
this.vertices = res;
}
}
Sample Code
Commentaires