Computer Graphics & Visualization @ufrj.br
Physical animation with shape matching
Also listed in…
Computer Graphics
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;
}
}
g = 0.2 // Gravity
shapeMatchAttenuation = 0.98
timeStepAttenuation = 0.98
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;
}
}
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] }];
}
vertexEdgeImpulse([0, 0], [-1, 1], [1, 1])
// 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;
}
// 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;
}
// 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]
)
}
// 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
}
// 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]
}
// Returns an array of 2d points ('pts') transformed matrix 'mat'
function transformPoints (pts,mat) {
return pts.map(p => vec2.transformMat2d([],p,mat))
}
//
// 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]]))
}
// 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)
}
// 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;
}
// 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;
}
// 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)
}
// 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);
}
// 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
}
// 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]
}
// 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)];
}
// 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)]]
}
// 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]]]
}
// Matrix transpose
function mat2transpose (m) {
return [[m[0][0],m[1][0]],
[m[0][1],m[1][1]]]
}
// Matrix determinant
function mat2det ([[a,b],[c,d]]) {
return a*d-b*c
}
// 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]]
}
// Matrix inverse
function mat2inv ([[a,b],[c,d]]) {
let det = a*d-b*c;
return [[d/det,-c/det],[-b/det,a/det]]
}
glmatrix = import('https://unpkg.com/gl-matrix@3.3.0/esm/index.js?module')
mat2d = glmatrix.mat2d
vec2 = glmatrix.vec2
import { gjk, epa, supportEdge } from "@esperanc/2d-gjk-and-epa-algorithms"
Purpose-built for displays of data
Observable is your go-to platform for exploring data and creating expressive data visualizations. Use reactive JavaScript notebooks for prototyping and a collaborative canvas for visual data exploration and dashboard creation.
