Published
Edited
Jan 6, 2021
9 stars
Insert cell
Insert cell
Insert cell
demo = {
reset;
let dsp = new Display();
let bodies = [];

function refresh() {
dsp.clear();
for (let body of bodies) {
dsp.drawPolygon(body.worldShape);
}
}

dsp.canvas.onclick = e => {
bodies.push(
new Body(
regularPolygonPoints([0, 0], 40, ~~(Math.random() * 3 + 3)),
[e.offsetX, e.offsetY],
Math.PI / 4
)
);
};

function physics() {
// Integrate
bodies.forEach(body => body.timeStep());

// Project collisions among objects and with world box
for (let irelax = 0; irelax < 10; irelax++) {
bodies.forEach(body =>
body.projectWalls([0, 0], [dsp.canvas.width, dsp.canvas.height])
);
bodies.forEach(body => body.shapeMatch());
let pairs = sap(bodies.map(body => body.worldShape), [1, 0]);

bodies.forEach((body, i) => {
pairs[i].forEach(j => body.projectCollision(bodies[j]), irelax);
});
bodies.forEach(body => body.shapeMatch());
}

bodies.forEach(body => {
if (body.collisionShape) body.updateFromShape(body.collisionShape);
});
}

for (let frame = 0; ; frame++) {
await visibility();
physics();
refresh();
yield dsp.canvas;
}
}
Insert cell
g = 0.2 // Gravity
Insert cell
shapeMatchAttenuation = 0.98
Insert cell
timeStepAttenuation = 0.98
Insert cell
class Body {
// Constructor. Shape is an array of 2d points
constructor(shape, pos = [0, 0], ang = 0, mass = 1) {
this.mass = mass;
this.pos = this.oldPos = pos;
this.ang = this.oldAng = ang;
let center = centerOfMass(shape);
this.shape = shape.map(p => [p[0] - center[0], p[1] - center[1]]);
this.dirty = true; // Tells if worldShape must be recomputed
this.collisionShape = null; // Non-null if collision was detected
}

// Verlet integration
timeStep() {
let vel = vec2.sub([], this.pos, this.oldPos);
vel[1] += g; // Add gravity to velocity
this.oldPos = this.pos;
this.pos = vec2.add([], this.pos, vec2.scale([], vel, timeStepAttenuation));
let angVel = this.ang - this.oldAng;
this.oldAng = this.ang;
this.ang += angVel * timeStepAttenuation;
this.dirty = true;
}

// Returns the shape points in world space
get worldShape() {
if (this.dirty) {
// Memoization of the result
let [c, s] = [Math.cos(this.ang), Math.sin(this.ang)];
let transf = mat2d.fromValues(c, s, -s, c, this.pos[0], this.pos[1]);
this.curWorldShape = transformPoints(this.shape, transf);
this.dirty = false;
}
return this.curWorldShape;
}

//=====================================================================
// Methods for collision response with shape matching
//

// Projects points of worldShape onto the walls of a rectangle
projectWalls(min, max) {
let projected = false;
let poly = this.collisionShape || this.worldShape.slice();
poly.forEach((p, i) => {
for (let [a, b, c] of [
[-1, 0, min[0]],
[1, 0, -max[0]],
[0, -1, min[1]],
[0, 1, -max[1]]
]) {
if (a * p[0] + b * p[1] + c >= 0) {
p = projectPointLine(p, a, b, c);
projected = true;
}
}
poly[i] = p;
});
if (projected) this.collisionShape = poly;
}

// Tests for collision between this body and other, and if true,
// performs a projection this object's collisionShape by computing a contact point
// using the collision geometry. Updates other as well
projectCollision(other, gravityBias = 0) {
let a = this.collisionShape || this.worldShape.slice();
let b = other.collisionShape || other.worldShape.slice();
let hit = gjk(a, b);
if (hit) {
let { p, q, n } = epa(a, b, ...hit);

let aPoints = supportEdge(a, n);
let bPoints = supportEdge(b, [-n[0], -n[1]]);

let [massA, massB] = [this.mass, other.mass];
if (gravityBias) {
if (this.pos[1] > other.pos[1]) massA += massB * gravityBias;
else massB += massA * gravityBias;
}

let aContact, bContact, aContactDisplaced, bContactDisplaced, penetration;
if (aPoints.length + bPoints.length == 4) {
// Edge-edge collision
let center = centerOfMass([...aPoints, ...bPoints]);
aContact = closestSegmentPoint(center, ...aPoints);
bContact = closestSegmentPoint(center, ...bPoints);
penetration = vec2.dist(aContact, bContact);
aContactDisplaced = vec2.add(
[],
aContact,
vec2.scale([], n, (-penetration * massA) / (massA + massB))
);
bContactDisplaced = vec2.add(
[],
bContact,
vec2.scale([], n, (penetration * massB) / (massA + massB))
);
this.curWorldShape.push(aContact);
a.push(aContactDisplaced);
other.curWorldShape.push(bContact);
b.push(bContactDisplaced);
} else {
// Vertex-edge collision
if (aPoints.length + bPoints.length != 3) {
console.log({ aPoints, bPoints });
throw "Weird collision";
}
if (aPoints.length == 2) {
aContact = closestSegmentPoint(bPoints[0], ...aPoints);
penetration = vec2.dist(aContact, bPoints[0]);
aContactDisplaced = vec2.add(
[],
aContact,
vec2.scale([], n, (-penetration * massA) / (massA + massB))
);
this.curWorldShape.push(aContact);
a.push(aContactDisplaced);
bContactDisplaced = vec2.add(
[],
bPoints[0],
vec2.scale([], n, (penetration * massB) / (massA + massB))
);
b.splice(b.lastIndexOf(bPoints[0]), 1, bContactDisplaced);
} else {
// bPoints.length == 2!
bContact = closestSegmentPoint(aPoints[0], ...bPoints);
penetration = vec2.dist(aPoints[0], bContact);
bContactDisplaced = vec2.add(
[],
bContact,
vec2.scale([], n, (penetration * massB) / (massA + massB))
);
other.curWorldShape.push(bContact);
b.push(bContactDisplaced);
aContactDisplaced = vec2.add(
[],
aPoints[0],
vec2.scale([], n, (-penetration * massA) / (massA + massB))
);
a.splice(a.lastIndexOf(aPoints[0]), 1, aContactDisplaced);
}
}
this.collisionShape = a;
other.collisionShape = b;
}
}

// Shape matches collision shape with the original shape and applies
// the resulting rigid transformation to collisionShape
shapeMatch() {
if (this.collisionShape) {
let M = shapeMatch(this.worldShape, this.collisionShape);
this.dirty = true;
this.collisionShape = transformPoints(this.worldShape, M);
}
}

// Updates rotation and position from the given shape
updateFromShape(shape) {
// New center of mass
let center = centerOfMass(shape);

// Rotation component
let rot = shapeMatchRotation(
this.worldShape.map(p => vec2.sub([], p, this.pos)),
shape.map(p => vec2.sub([], p, center))
);
if (!Number.isNaN(rot[0][0])) {
// Avoid degenerate projections
let dang = Math.atan2(rot[0][1], rot[0][0]);
this.ang += dang * shapeMatchAttenuation;
}

// Translation component
this.pos = vec2.lerp([], this.pos, center, shapeMatchAttenuation);

// This is the new worldShape
this.curWorldShape = shape;
this.collisionShape = null;
this.dirty = false;
}

}
Insert cell
function vertexEdgeImpulse(v, p, q) {
let u = closestSegmentPoint(v, p, q);
let [dx, dy] = [(u[0] - v[0]) / 2, (u[1] - v[1]) / 2];
return [{ p: v, d: [dx, dy] }, { p: u, d: [-dx, -dy] }];
}
Insert cell
vertexEdgeImpulse([0, 0], [-1, 1], [1, 1])
Insert cell
Insert cell
// Projects back points of poly outside the rectangle with one corner at the origin and
// thei given width and height.
// Returns true if projection was performed on poly, in which case,
// the vertices of poly are modified in place.
function projectHalfspaces(poly, width, height) {
let projected = false;
for (let i = 0; i < poly.length; i++) {
let p = poly[i];
for (let [a, b, c] of [
[-1, 0, 0],
[1, 0, -width],
[0, -1, 0],
[0, 1, -height]
]) {
if (a * p[0] + b * p[1] + c >= 0) {
p = projectPointLine(p, a, b, c);
projected = true;
}
}
poly[i] = p;
}
return projected;
}
Insert cell
// A simple sweep-and-prune scheme. Detects collisions by projecting on vector v.
// Returns an array of lists 'pairs', such that pairs[i] contains the indices
// of all polygons that intersect pairs[i].
function sap(polygons, v) {
let n = polygons.length;
let pairs = [];
let proj = [];
polygons.forEach((poly, i) => {
let min = Number.POSITIVE_INFINITY;
let max = Number.NEGATIVE_INFINITY;
for (let p of poly) {
let dot = vec2.dot(p, v);
min = Math.min(min, dot);
max = Math.max(max, dot);
}
proj.push([min, i], [max, i]);
});
proj.sort((a, b) => a[0] - b[0]);

let inside = new Set();
for (let [_, i] of proj) {
if (inside.has(i)) {
inside.delete(i);
} else {
pairs[i] = [];
for (let j of inside) {
if (i < j) pairs[j].push(i);
else pairs[i].push(j);
}
inside.add(i);
}
}
return pairs;
}
Insert cell
// Returns an array of 4 points, one for each corner of a rectangle
// having the given center and size
function rectanglePoints (center = [0,0], size = [1,1]) {
return [[-0.5,-0.5],[0.5,-0.5],[0.5,0.5],[-0.5,0.5]].map(
([dx,dy]) =>[center[0]+size[0]*dx,center[1]+size[1]*dy]
)
}
Insert cell
// Returns an array of points with the vertices of a regular polygon
// centered at 'center', with the given radius and number of sides.
function regularPolygonPoints (center = [0,0], radius = 1, nsides=3) {
let poly = [];
let delta = Math.PI*2/nsides;
for (let i = 0; i < nsides; i++) {
poly.push([center[0]+radius*Math.cos(delta*i),center[1]+radius*Math.sin(delta*i)])
}
return poly
}
Insert cell
// Returns the center of mass of an array of 2d points
function centerOfMass (pts) {
let sum = pts.reduce((a,b) => [a[0]+b[0],a[1]+b[1]])
return [sum[0]/pts.length,sum[1]/pts.length]
}
Insert cell
// Returns an array of 2d points ('pts') transformed matrix 'mat'
function transformPoints (pts,mat) {
return pts.map(p => vec2.transformMat2d([],p,mat))
}
Insert cell
//
// Given an array of source points and an array of destination points as in Fig 3
// of the paper, returns a rigid transformation matrix that best approximates the
// transformation from source to destination points.
//
function shapeMatch (srcPoints, dstPoints) {
// The center of mass of the src points
let srcCenter = centerOfMass(srcPoints)
// src displacement vectors wrt center of mass
let srcVectors = srcPoints.map (p => vec2.sub([],p,srcCenter))
// destination points center of mass
let dstCenter = centerOfMass(dstPoints)
// dst displacement vectors wrt center of mass
let dstVectors = dstPoints.map (p => vec2.sub([],p,dstCenter))
// Compute rotation and compose with the two translations
let [[a,b],[c,d]] = shapeMatchRotation (srcVectors,dstVectors);
return mat2d.mul([],
mat2d.fromValues(a,b,
c,d,
dstCenter[0],dstCenter[1]),
mat2d.fromTranslation([],[-srcCenter[0],-srcCenter[1]]))
}
Insert cell
// Solves the rotation part of the shapeMatching problem.
// Given the source and destination vectors, i.e., the points with the translation
// factored out, returns the rotation transformation (a 2x2 matrix)
function shapeMatchRotation (srcVectors, dstVectors) {
// function that computes p x q^T
let pqT = (p,q) => [[p[0]*q[0],p[0]*q[1]],
[p[1]*q[0],p[1]*q[1]]];
// Eq 7
let Apq = srcVectors.map((p,i) => pqT(p,dstVectors[i])).reduce(mat2sum)
let ApqTxApq = mat2mul(mat2transpose(Apq),Apq);
let S = mat2sqrt(ApqTxApq)
let Sinv = mat2inv(S);
return mat2mul(Apq,Sinv)
}
Insert cell
// Tests collisions among polygons and returns projected points for those
// in collision. Returns an array 'newPolygons' where newPolygons [i] is a
// projected copy of polygons[i] if it is in collision or null otherwise.
function projectCollisions(polygons) {
let npoly = polygons.length;
let newPolygons = [];

// Enhance performance using sweep and prune
let pairs = sap(polygons, [1, 0]);

for (let i = 0; i < npoly; i++) {
let a = polygons[i];
//for (let j = i + 1; j < npoly; j++) {
for (let j of pairs[i]) {
let b = polygons[j];
let hit = gjk(a, b);
if (hit) {
let { p, q, n } = epa(a, b, ...hit);
let ptsA = supportEdge(a, n);
let ptsB = supportEdge(b, [-n[0], -n[1]]);
let center = centerOfMass(ptsA.concat(ptsB));
let [ha, hb, hc] = halfSpace(center, n);
let acopy = newPolygons[i] || a.slice();
acopy.forEach((pt, i) => {
if (ha * pt[0] + hb * pt[1] + hc > 0)
acopy[i] = projectPointLine(pt, ha, hb, hc);
});
newPolygons[i] = acopy;
let bcopy = newPolygons[j] || b.slice();
bcopy.forEach((pt, i) => {
if (ha * pt[0] + hb * pt[1] + hc < 0)
bcopy[i] = projectPointLine(pt, ha, hb, hc);
});
newPolygons[j] = bcopy;
}
}
}
return newPolygons;
}
Insert cell
// Performs one step of the polygon separation algorithm. Changes
// the 'polygons' array in place, moving them using the shape matching approach.
// Returns true iff collision was detected
function separatePolygons (polygons) {
let newPolys = projectCollisions(polygons);
let collide = false;
for (let i = 0; i < polygons.length; i++) {
if (newPolys [i]) {
let M = shapeMatch (polygons[i],newPolys[i]);
polygons[i] = transformPoints(polygons[i],M);
collide = true;
}
}
return collide;
}
Insert cell
// Returns the point on line segment q-r closest to point p
function closestSegmentPoint(p, q, r) {
let s = vec2.dist(q,r);
if (s < 0.00001) return q; // Degenerate line segment - both endpoints at approximately the same distance
let v = vec2.normalize([],vec2.sub([],r,q));
let u = vec2.sub([],p,q);
let d = vec2.dot(u,v);
if (d < 0) return q;
if (d > s) return r;
return vec2.lerp ([], q, r, d/s)
}
Insert cell
// returns a point on line p-q that is dist units away from p. If dist
// is positive, the result in the same direction from p as q, otherwise it is
// in the opposite direction
function pointAtDist (p,q,dist) {
let v = vec2.normalize([], vec2.sub([],q,p));
vec2.scale(v,v,dist)
return vec2.add([],p,v);
}
Insert cell
// Returns true iff p inside poly
function pointInPolygon (p, poly) {
let [x,y] = p;
let xLeft = Number.NEGATIVE_INFINITY, xRight = Number.POSITIVE_INFINITY;
let leftSign = 0, rightSign = 0;
let q = poly[poly.length-1];
for (let p of poly) {
if ((q[1] < y) != (p[1] < y)) {
let alpha = (p[1]-y)/(p[1]-q[1])
let xi = p[0]*(1-alpha)+q[0]*alpha;
if (xi < x) {
if (xi > xLeft) {
xLeft = xi;
leftSign = Math.sign(p[1]-q[1])
}
}
else {
if (xi < xRight) {
xRight = xi;
rightSign = Math.sign(p[1]-q[1])
}
}
}
q = p;
}
return xLeft != Number.NEGATIVE_INFINITY && xRight != Number.POSITIVE_INFINITY && leftSign < 0
}
Insert cell
// Given a line given by a*p[0] + b*p[1] + c = 0, and a point p, returns the orthogonal projection
// of p onto the line
function projectPointLine (p, a, b, c) {
if (a == 0) return [p[0],-c/b];
if (b == 0) return [-c/a,p[1]];
let d = b*p[0]-a*p[1];
let y = (-a*d-c*b)/(a*a+b*b);
let x = (b*y+c)/-a;
return [x,y]
}
Insert cell
// Returns [a,b,c], representing the line passing through 'point' and having the given 'normal'.
function halfSpace(point, normal) {
console.assert(!isNaN(normal[0]));
return [normal[0], normal[1], -vec2.dot(point, normal)];
}
Insert cell
Insert cell
Insert cell
Insert cell
// Matrix multiplication
function mat2mul(a,b) {
let prod = (i,j) => a[i][0]*b[0][j]+a[i][1]*b[1][j];
return [[prod(0,0),prod(0,1)],
[prod(1,0),prod(1,1)]]
}
Insert cell
// Matrix sum
function mat2sum (m1,m2) {
return [[m1[0][0]+m2[0][0],m1[0][1]+m2[0][1]],
[m1[1][0]+m2[1][0],m1[1][1]+m2[1][1]]]
}
Insert cell
// Matrix transpose
function mat2transpose (m) {
return [[m[0][0],m[1][0]],
[m[0][1],m[1][1]]]
}
Insert cell
// Matrix determinant
function mat2det ([[a,b],[c,d]]) {
return a*d-b*c
}
Insert cell
// Matrix square root
function mat2sqrt ([[a,b],[c,d]]) {
let s = Math.sqrt(a*d-b*c);
let t = Math.sqrt(a+d+2*s);
return [[(a+s)/t,b/t],[c/t,(d+s)/t]]
}
Insert cell
// Matrix inverse
function mat2inv ([[a,b],[c,d]]) {
let det = a*d-b*c;
return [[d/det,-c/det],[-b/det,a/det]]
}
Insert cell
Insert cell
glmatrix = import('https://unpkg.com/gl-matrix@3.3.0/esm/index.js?module')
Insert cell
mat2d = glmatrix.mat2d
Insert cell
vec2 = glmatrix.vec2
Insert cell
import { gjk, epa, supportEdge } from "@esperanc/2d-gjk-and-epa-algorithms"
Insert cell

One platform to build and deploy the best data apps

Experiment and prototype by building visualizations in live JavaScript notebooks. Collaborate with your team and decide which concepts to build out.
Use Observable Framework to build data apps locally. Use data loaders to build in any language or library, including Python, SQL, and R.
Seamlessly deploy to Observable. Test before you ship, use automatic deploy-on-commit, and ensure your projects are always up-to-date.
Learn more