Published
Edited
Dec 20, 2021
1 fork
Importers
11 stars
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
// A simple implementation of the Sweep-And-Prune broad phase collision detection scheme
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
// How many relaxation iterations
relaxIterations = 10
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
// Returns a rigid transformation matrix by composing a rotation (given by 'angle') around a point
// (given by 'center') with a translation given by 'vector'. Note that rotation is applied first.
function rigidTransf (center = [0,0], angle = 0, vector =[0,0]) {
return mat2d.mul([],
mat2d.fromTranslation([],[center[0]+vector[0],center[1]+vector[1]]),
mat2d.mul([],
mat2d.fromRotation([],angle),
mat2d.fromTranslation([],[-center[0],-center[1]])
)
)
}
Insert cell
glmatrix.mat2.determinant([Math.cos(1), Math.sin(1), -Math.sin(1), Math.cos(1)])
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);
let det = glmatrix.mat2.determinant([a, b, c, d]);
if (isNaN(det) || det < 0.9 || det > 1.1) {
console.log("BUG shapeMatch", { det, a, b, c, d, srcPoints, dstPoints });
a = d = 1;
b = c = 0;
//throw "BUG shapeMatch";
}

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);
console.assert(ptsA.length > 0);
let ptsB = supportEdge(b, [-n[0], -n[1]]);
console.assert(ptsB.length > 0);
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