class Body {
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;
this.collisionShape = null;
}
timeStep() {
let vel = vec2.sub([], this.pos, this.oldPos);
vel[1] += g;
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;
}
get worldShape() {
if (this.dirty) {
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;
}
}