planck
Version:
2D JavaScript/TypeScript physics engine for cross-platform HTML5 game development
1,409 lines (1,178 loc) • 44.8 kB
text/typescript
/*
* Planck.js
*
* Copyright (c) Erin Catto, Ali Shakiba
*
* This source code is licensed under the MIT license found in the
* LICENSE file in the root directory of this source tree.
*/
import * as matrix from "../common/Matrix";
import { ShapeType } from "../collision/Shape";
import { clamp } from "../common/Math";
import { TransformValue } from "../common/Transform";
import { Mat22 } from "../common/Mat22";
import { SettingsInternal as Settings } from "../Settings";
import { Manifold, ManifoldType, WorldManifold } from "../collision/Manifold";
import { testOverlap } from "../collision/Distance";
import { Fixture } from "./Fixture";
import { Body } from "./Body";
import { ContactImpulse, TimeStep } from "./Solver";
import { Pool } from "../util/Pool";
import { getTransform } from "./Position";
/** @internal */ const _ASSERT = typeof ASSERT === "undefined" ? false : ASSERT;
/** @internal */ const math_abs = Math.abs;
/** @internal */ const math_sqrt = Math.sqrt;
/** @internal */ const math_max = Math.max;
/** @internal */ const math_min = Math.min;
// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix.
/** @internal */ const DEBUG_SOLVER = false;
/** @internal */ const contactPool = new Pool<Contact>({
create() {
return new Contact();
},
release(contact: Contact) {
contact.recycle();
},
});
/** @internal */ const oldManifold = new Manifold();
/** @internal */ const worldManifold = new WorldManifold();
/**
* A contact edge is used to connect bodies and contacts together in a contact
* graph where each body is a node and each contact is an edge. A contact edge
* belongs to a doubly linked list maintained in each attached body. Each
* contact has two contact nodes, one for each attached body.
*/
export class ContactEdge {
contact: Contact;
prev: ContactEdge | null = null;
next: ContactEdge | null = null;
other: Body | null = null;
constructor(contact: Contact) {
this.contact = contact;
}
/** @internal */
recycle() {
this.prev = null;
this.next = null;
this.other = null;
}
}
export type EvaluateFunction = (
manifold: Manifold,
xfA: TransformValue,
fixtureA: Fixture,
indexA: number,
xfB: TransformValue,
fixtureB: Fixture,
indexB: number,
) => void;
/**
* Friction mixing law. The idea is to allow either fixture to drive the
* friction to zero. For example, anything slides on ice.
*/
export function mixFriction(friction1: number, friction2: number): number {
return math_sqrt(friction1 * friction2);
}
/**
* Restitution mixing law. The idea is allow for anything to bounce off an
* inelastic surface. For example, a superball bounces on anything.
*/
export function mixRestitution(restitution1: number, restitution2: number): number {
return restitution1 > restitution2 ? restitution1 : restitution2;
}
// TODO: move this to Settings?
/** @internal */ const s_registers = [];
// TODO: merge with ManifoldPoint?
export class VelocityConstraintPoint {
rA = matrix.vec2(0, 0);
rB = matrix.vec2(0, 0);
normalImpulse = 0;
tangentImpulse = 0;
normalMass = 0;
tangentMass = 0;
velocityBias = 0;
recycle() {
matrix.zeroVec2(this.rA);
matrix.zeroVec2(this.rB);
this.normalImpulse = 0;
this.tangentImpulse = 0;
this.normalMass = 0;
this.tangentMass = 0;
this.velocityBias = 0;
}
}
/** @internal */ const cA = matrix.vec2(0, 0);
/** @internal */ const vA = matrix.vec2(0, 0);
/** @internal */ const cB = matrix.vec2(0, 0);
/** @internal */ const vB = matrix.vec2(0, 0);
/** @internal */ const tangent = matrix.vec2(0, 0);
/** @internal */ const xfA = matrix.transform(0, 0, 0);
/** @internal */ const xfB = matrix.transform(0, 0, 0);
/** @internal */ const pointA = matrix.vec2(0, 0);
/** @internal */ const pointB = matrix.vec2(0, 0);
/** @internal */ const clipPoint = matrix.vec2(0, 0);
/** @internal */ const planePoint = matrix.vec2(0, 0);
/** @internal */ const rA = matrix.vec2(0, 0);
/** @internal */ const rB = matrix.vec2(0, 0);
/** @internal */ const P = matrix.vec2(0, 0);
/** @internal */ const normal = matrix.vec2(0, 0);
/** @internal */ const point = matrix.vec2(0, 0);
/** @internal */ const dv = matrix.vec2(0, 0);
/** @internal */ const dv1 = matrix.vec2(0, 0);
/** @internal */ const dv2 = matrix.vec2(0, 0);
/** @internal */ const b = matrix.vec2(0, 0);
/** @internal */ const a = matrix.vec2(0, 0);
/** @internal */ const x = matrix.vec2(0, 0);
/** @internal */ const d = matrix.vec2(0, 0);
/** @internal */ const P1 = matrix.vec2(0, 0);
/** @internal */ const P2 = matrix.vec2(0, 0);
/** @internal */ const temp = matrix.vec2(0, 0);
/**
* The class manages contact between two shapes. A contact exists for each
* overlapping AABB in the broad-phase (except if filtered). Therefore a contact
* object may exist that has no contact points.
*/
export class Contact {
// Nodes for connecting bodies.
/** @internal */ m_nodeA = new ContactEdge(this);
/** @internal */ m_nodeB = new ContactEdge(this);
/** @internal */ m_fixtureA: Fixture | null = null;
/** @internal */ m_fixtureB: Fixture | null = null;
/** @internal */ m_indexA = -1;
/** @internal */ m_indexB = -1;
/** @internal */ m_evaluateFcn: EvaluateFunction | null = null;
/** @internal */ m_manifold: Manifold = new Manifold();
/** @internal */ m_prev: Contact | null = null;
/** @internal */ m_next: Contact | null = null;
/** @internal */ m_toi = 1.0;
/** @internal */ m_toiCount = 0;
// This contact has a valid TOI in m_toi
/** @internal */ m_toiFlag = false;
/** @internal */ m_friction = 0.0;
/** @internal */ m_restitution = 0.0;
/** @internal */ m_tangentSpeed = 0.0;
/** @internal This contact can be disabled (by user) */
m_enabledFlag = true;
/** @internal Used when crawling contact graph when forming islands. */
m_islandFlag = false;
/** @internal Set when the shapes are touching. */
m_touchingFlag = false;
/** @internal This contact needs filtering because a fixture filter was changed. */
m_filterFlag = false;
/** @internal This bullet contact had a TOI event */
m_bulletHitFlag = false;
/** @internal Contact reporting impulse object cache */
m_impulse: ContactImpulse = new ContactImpulse(this);
// VelocityConstraint
/** @internal */ v_points = [new VelocityConstraintPoint(), new VelocityConstraintPoint()]; // [maxManifoldPoints];
/** @internal */ v_normal = matrix.vec2(0, 0);
/** @internal */ v_normalMass: Mat22 = new Mat22();
/** @internal */ v_K: Mat22 = new Mat22();
/** @internal */ v_pointCount = 0;
/** @internal */ v_tangentSpeed = 0;
/** @internal */ v_friction = 0;
/** @internal */ v_restitution = 0;
/** @internal */ v_invMassA = 0;
/** @internal */ v_invMassB = 0;
/** @internal */ v_invIA = 0;
/** @internal */ v_invIB = 0;
// PositionConstraint
/** @internal */ p_localPoints = [matrix.vec2(0, 0), matrix.vec2(0, 0)]; // [maxManifoldPoints];
/** @internal */ p_localNormal = matrix.vec2(0, 0);
/** @internal */ p_localPoint = matrix.vec2(0, 0);
/** @internal */ p_localCenterA = matrix.vec2(0, 0);
/** @internal */ p_localCenterB = matrix.vec2(0, 0);
/** @internal */ p_type = ManifoldType.e_unset;
/** @internal */ p_radiusA = 0;
/** @internal */ p_radiusB = 0;
/** @internal */ p_pointCount = 0;
/** @internal */ p_invMassA = 0;
/** @internal */ p_invMassB = 0;
/** @internal */ p_invIA = 0;
/** @internal */ p_invIB = 0;
/** @internal */
initialize(fA: Fixture, indexA: number, fB: Fixture, indexB: number, evaluateFcn: EvaluateFunction) {
this.m_fixtureA = fA;
this.m_fixtureB = fB;
this.m_indexA = indexA;
this.m_indexB = indexB;
this.m_evaluateFcn = evaluateFcn;
this.m_friction = mixFriction(this.m_fixtureA.m_friction, this.m_fixtureB.m_friction);
this.m_restitution = mixRestitution(this.m_fixtureA.m_restitution, this.m_fixtureB.m_restitution);
}
/** @internal */
recycle() {
this.m_nodeA.recycle();
this.m_nodeB.recycle();
this.m_fixtureA = null;
this.m_fixtureB = null;
this.m_indexA = -1;
this.m_indexB = -1;
this.m_evaluateFcn = null;
this.m_manifold.recycle();
this.m_prev = null;
this.m_next = null;
this.m_toi = 1;
this.m_toiCount = 0;
this.m_toiFlag = false;
this.m_friction = 0;
this.m_restitution = 0;
this.m_tangentSpeed = 0;
this.m_enabledFlag = true;
this.m_islandFlag = false;
this.m_touchingFlag = false;
this.m_filterFlag = false;
this.m_bulletHitFlag = false;
this.m_impulse.recycle();
// VelocityConstraint
for (const point of this.v_points) {
point.recycle();
}
matrix.zeroVec2(this.v_normal);
this.v_normalMass.setZero();
this.v_K.setZero();
this.v_pointCount = 0;
this.v_tangentSpeed = 0;
this.v_friction = 0;
this.v_restitution = 0;
this.v_invMassA = 0;
this.v_invMassB = 0;
this.v_invIA = 0;
this.v_invIB = 0;
// PositionConstraint
for (const point of this.p_localPoints) {
matrix.zeroVec2(point);
}
matrix.zeroVec2(this.p_localNormal);
matrix.zeroVec2(this.p_localPoint);
matrix.zeroVec2(this.p_localCenterA);
matrix.zeroVec2(this.p_localCenterB);
this.p_type = ManifoldType.e_unset;
this.p_radiusA = 0;
this.p_radiusB = 0;
this.p_pointCount = 0;
this.p_invMassA = 0;
this.p_invMassB = 0;
this.p_invIA = 0;
this.p_invIB = 0;
}
initConstraint(step: TimeStep): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const shapeA = fixtureA.m_shape;
const shapeB = fixtureB.m_shape;
if (shapeA === null || shapeB === null) return;
const manifold = this.m_manifold;
const pointCount = manifold.pointCount;
if (_ASSERT) console.assert(pointCount > 0);
this.v_invMassA = bodyA.m_invMass;
this.v_invMassB = bodyB.m_invMass;
this.v_invIA = bodyA.m_invI;
this.v_invIB = bodyB.m_invI;
this.v_friction = this.m_friction;
this.v_restitution = this.m_restitution;
this.v_tangentSpeed = this.m_tangentSpeed;
this.v_pointCount = pointCount;
this.v_K.setZero();
this.v_normalMass.setZero();
this.p_invMassA = bodyA.m_invMass;
this.p_invMassB = bodyB.m_invMass;
this.p_invIA = bodyA.m_invI;
this.p_invIB = bodyB.m_invI;
matrix.copyVec2(this.p_localCenterA, bodyA.m_sweep.localCenter);
matrix.copyVec2(this.p_localCenterB, bodyB.m_sweep.localCenter);
this.p_radiusA = shapeA.m_radius;
this.p_radiusB = shapeB.m_radius;
this.p_type = manifold.type;
matrix.copyVec2(this.p_localNormal, manifold.localNormal);
matrix.copyVec2(this.p_localPoint, manifold.localPoint);
this.p_pointCount = pointCount;
for (let j = 0; j < Settings.maxManifoldPoints; ++j) {
this.v_points[j].recycle();
matrix.zeroVec2(this.p_localPoints[j]);
}
for (let j = 0; j < pointCount; ++j) {
const cp = manifold.points[j];
const vcp = this.v_points[j];
if (step.warmStarting) {
vcp.normalImpulse = step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = step.dtRatio * cp.tangentImpulse;
}
matrix.copyVec2(this.p_localPoints[j], cp.localPoint);
}
}
/**
* Get the contact manifold. Do not modify the manifold unless you understand
* the internals of the library.
*/
getManifold(): Manifold {
return this.m_manifold;
}
/**
* Get the world manifold.
*/
getWorldManifold(worldManifold: WorldManifold | null): WorldManifold | undefined {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const shapeA = fixtureA.m_shape;
const shapeB = fixtureB.m_shape;
if (shapeA === null || shapeB === null) return;
return this.m_manifold.getWorldManifold(
worldManifold,
bodyA.getTransform(),
shapeA.m_radius,
bodyB.getTransform(),
shapeB.m_radius,
);
}
/**
* Enable/disable this contact. This can be used inside the pre-solve contact
* listener. The contact is only disabled for the current time step (or sub-step
* in continuous collisions).
*/
setEnabled(flag: boolean): void {
this.m_enabledFlag = !!flag;
}
/**
* Has this contact been disabled?
*/
isEnabled(): boolean {
return this.m_enabledFlag;
}
/**
* Is this contact touching?
*/
isTouching(): boolean {
return this.m_touchingFlag;
}
/**
* Get the next contact in the world's contact list.
*/
getNext(): Contact | null {
return this.m_next;
}
/**
* Get fixture A in this contact.
*/
getFixtureA(): Fixture {
return this.m_fixtureA;
}
/**
* Get fixture B in this contact.
*/
getFixtureB(): Fixture {
return this.m_fixtureB;
}
/**
* Get the child primitive index for fixture A.
*/
getChildIndexA(): number {
return this.m_indexA;
}
/**
* Get the child primitive index for fixture B.
*/
getChildIndexB(): number {
return this.m_indexB;
}
/**
* Flag this contact for filtering. Filtering will occur the next time step.
*/
flagForFiltering(): void {
this.m_filterFlag = true;
}
/**
* Override the default friction mixture. You can call this in
* "pre-solve" callback. This value persists until set or reset.
*/
setFriction(friction: number): void {
this.m_friction = friction;
}
/**
* Get the friction.
*/
getFriction(): number {
return this.m_friction;
}
/**
* Reset the friction mixture to the default value.
*/
resetFriction(): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
this.m_friction = mixFriction(fixtureA.m_friction, fixtureB.m_friction);
}
/**
* Override the default restitution mixture. You can call this in
* "pre-solve" callback. The value persists until you set or reset.
*/
setRestitution(restitution: number): void {
this.m_restitution = restitution;
}
/**
* Get the restitution.
*/
getRestitution(): number {
return this.m_restitution;
}
/**
* Reset the restitution to the default value.
*/
resetRestitution(): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
this.m_restitution = mixRestitution(fixtureA.m_restitution, fixtureB.m_restitution);
}
/**
* Set the desired tangent speed for a conveyor belt behavior. In meters per
* second.
*/
setTangentSpeed(speed: number): void {
this.m_tangentSpeed = speed;
}
/**
* Get the desired tangent speed. In meters per second.
*/
getTangentSpeed(): number {
return this.m_tangentSpeed;
}
/**
* Called by Update method, and implemented by subclasses.
*/
evaluate(manifold: Manifold, xfA: TransformValue, xfB: TransformValue): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
this.m_evaluateFcn(manifold, xfA, fixtureA, this.m_indexA, xfB, fixtureB, this.m_indexB);
}
/**
* Updates the contact manifold and touching status.
*
* Note: do not assume the fixture AABBs are overlapping or are valid.
*
* @param listener.beginContact
* @param listener.endContact
* @param listener.preSolve
*/
update(listener?: {
beginContact(contact: Contact): void;
endContact(contact: Contact): void;
preSolve(contact: Contact, oldManifold: Manifold): void;
}): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const shapeA = fixtureA.m_shape;
const shapeB = fixtureB.m_shape;
if (shapeA === null || shapeB === null) return;
// Re-enable this contact.
this.m_enabledFlag = true;
let touching = false;
const wasTouching = this.m_touchingFlag;
const sensorA = fixtureA.m_isSensor;
const sensorB = fixtureB.m_isSensor;
const sensor = sensorA || sensorB;
const xfA = bodyA.m_xf;
const xfB = bodyB.m_xf;
// Is this contact a sensor?
if (sensor) {
touching = testOverlap(shapeA, this.m_indexA, shapeB, this.m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
this.m_manifold.pointCount = 0;
} else {
oldManifold.recycle();
oldManifold.set(this.m_manifold);
this.m_manifold.recycle();
this.evaluate(this.m_manifold, xfA, xfB);
touching = this.m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (let i = 0; i < this.m_manifold.pointCount; ++i) {
const nmp = this.m_manifold.points[i];
nmp.normalImpulse = 0.0;
nmp.tangentImpulse = 0.0;
for (let j = 0; j < oldManifold.pointCount; ++j) {
const omp = oldManifold.points[j];
if (omp.id.key === nmp.id.key) {
nmp.normalImpulse = omp.normalImpulse;
nmp.tangentImpulse = omp.tangentImpulse;
break;
}
}
}
if (touching !== wasTouching) {
bodyA.setAwake(true);
bodyB.setAwake(true);
}
}
this.m_touchingFlag = touching;
const hasListener = typeof listener === "object" && listener !== null;
if (!wasTouching && touching && hasListener) {
listener.beginContact(this);
}
if (wasTouching && !touching && hasListener) {
listener.endContact(this);
}
if (!sensor && touching && hasListener && oldManifold) {
listener.preSolve(this, oldManifold);
}
}
solvePositionConstraint(step: TimeStep): number {
return this._solvePositionConstraint(step, null, null);
}
solvePositionConstraintTOI(step: TimeStep, toiA: Body, toiB: Body): number {
return this._solvePositionConstraint(step, toiA, toiB);
}
private _solvePositionConstraint(step: TimeStep, toiA: Body | null, toiB: Body | null): number {
const toi = toiA !== null && toiB !== null ? true : false;
let minSeparation = 0.0;
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return minSeparation;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return minSeparation;
// const velocityA = bodyA.c_velocity;
// const velocityB = bodyB.c_velocity;
const positionA = bodyA.c_position;
const positionB = bodyB.c_position;
const localCenterA = this.p_localCenterA;
const localCenterB = this.p_localCenterB;
let mA = 0.0;
let iA = 0.0;
if (!toi || bodyA === toiA || bodyA === toiB) {
mA = this.p_invMassA;
iA = this.p_invIA;
}
let mB = 0.0;
let iB = 0.0;
if (!toi || bodyB === toiA || bodyB === toiB) {
mB = this.p_invMassB;
iB = this.p_invIB;
}
matrix.copyVec2(cA, positionA.c);
let aA = positionA.a;
matrix.copyVec2(cB, positionB.c);
let aB = positionB.a;
// Solve normal constraints
for (let j = 0; j < this.p_pointCount; ++j) {
getTransform(xfA, localCenterA, cA, aA);
getTransform(xfB, localCenterB, cB, aB);
// PositionSolverManifold
let separation: number;
switch (this.p_type) {
case ManifoldType.e_circles: {
matrix.transformVec2(pointA, xfA, this.p_localPoint);
matrix.transformVec2(pointB, xfB, this.p_localPoints[0]);
matrix.subVec2(normal, pointB, pointA);
matrix.normalizeVec2(normal);
matrix.combine2Vec2(point, 0.5, pointA, 0.5, pointB);
separation =
matrix.dotVec2(pointB, normal) - matrix.dotVec2(pointA, normal) - this.p_radiusA - this.p_radiusB;
break;
}
case ManifoldType.e_faceA: {
matrix.rotVec2(normal, xfA.q, this.p_localNormal);
matrix.transformVec2(planePoint, xfA, this.p_localPoint);
matrix.transformVec2(clipPoint, xfB, this.p_localPoints[j]);
separation =
matrix.dotVec2(clipPoint, normal) - matrix.dotVec2(planePoint, normal) - this.p_radiusA - this.p_radiusB;
matrix.copyVec2(point, clipPoint);
break;
}
case ManifoldType.e_faceB: {
matrix.rotVec2(normal, xfB.q, this.p_localNormal);
matrix.transformVec2(planePoint, xfB, this.p_localPoint);
matrix.transformVec2(clipPoint, xfA, this.p_localPoints[j]);
separation =
matrix.dotVec2(clipPoint, normal) - matrix.dotVec2(planePoint, normal) - this.p_radiusA - this.p_radiusB;
matrix.copyVec2(point, clipPoint);
// Ensure normal points from A to B
matrix.negVec2(normal);
break;
}
// todo: what should we do here?
default: {
return minSeparation;
}
}
matrix.subVec2(rA, point, cA);
matrix.subVec2(rB, point, cB);
// Track max constraint error.
minSeparation = math_min(minSeparation, separation);
const baumgarte = toi ? Settings.toiBaugarte : Settings.baumgarte;
const linearSlop = Settings.linearSlop;
const maxLinearCorrection = Settings.maxLinearCorrection;
// Prevent large corrections and allow slop.
const C = clamp(baumgarte * (separation + linearSlop), -maxLinearCorrection, 0.0);
// Compute the effective mass.
const rnA = matrix.crossVec2Vec2(rA, normal);
const rnB = matrix.crossVec2Vec2(rB, normal);
const K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
const impulse = K > 0.0 ? -C / K : 0.0;
matrix.scaleVec2(P, impulse, normal);
matrix.minusScaleVec2(cA, mA, P);
aA -= iA * matrix.crossVec2Vec2(rA, P);
matrix.plusScaleVec2(cB, mB, P);
aB += iB * matrix.crossVec2Vec2(rB, P);
}
matrix.copyVec2(positionA.c, cA);
positionA.a = aA;
matrix.copyVec2(positionB.c, cB);
positionB.a = aB;
return minSeparation;
}
initVelocityConstraint(step: TimeStep): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const velocityA = bodyA.c_velocity;
const velocityB = bodyB.c_velocity;
const positionA = bodyA.c_position;
const positionB = bodyB.c_position;
const radiusA = this.p_radiusA;
const radiusB = this.p_radiusB;
const manifold = this.m_manifold;
const mA = this.v_invMassA;
const mB = this.v_invMassB;
const iA = this.v_invIA;
const iB = this.v_invIB;
const localCenterA = this.p_localCenterA;
const localCenterB = this.p_localCenterB;
matrix.copyVec2(cA, positionA.c);
const aA = positionA.a;
matrix.copyVec2(vA, velocityA.v);
const wA = velocityA.w;
matrix.copyVec2(cB, positionB.c);
const aB = positionB.a;
matrix.copyVec2(vB, velocityB.v);
const wB = velocityB.w;
if (_ASSERT) console.assert(manifold.pointCount > 0);
getTransform(xfA, localCenterA, cA, aA);
getTransform(xfB, localCenterB, cB, aB);
worldManifold.recycle();
manifold.getWorldManifold(worldManifold, xfA, radiusA, xfB, radiusB);
matrix.copyVec2(this.v_normal, worldManifold.normal);
for (let j = 0; j < this.v_pointCount; ++j) {
const vcp = this.v_points[j]; // VelocityConstraintPoint
const wmp = worldManifold.points[j];
matrix.subVec2(vcp.rA, wmp, cA);
matrix.subVec2(vcp.rB, wmp, cB);
const rnA = matrix.crossVec2Vec2(vcp.rA, this.v_normal);
const rnB = matrix.crossVec2Vec2(vcp.rB, this.v_normal);
const kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0.0 ? 1.0 / kNormal : 0.0;
matrix.crossVec2Num(tangent, this.v_normal, 1.0);
const rtA = matrix.crossVec2Vec2(vcp.rA, tangent);
const rtB = matrix.crossVec2Vec2(vcp.rB, tangent);
const kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.tangentMass = kTangent > 0.0 ? 1.0 / kTangent : 0.0;
// Setup a velocity bias for restitution.
vcp.velocityBias = 0.0;
let vRel = 0;
vRel += matrix.dotVec2(this.v_normal, vB);
vRel += matrix.dotVec2(this.v_normal, matrix.crossNumVec2(temp, wB, vcp.rB));
vRel -= matrix.dotVec2(this.v_normal, vA);
vRel -= matrix.dotVec2(this.v_normal, matrix.crossNumVec2(temp, wA, vcp.rA));
if (vRel < -Settings.velocityThreshold) {
vcp.velocityBias = -this.v_restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (this.v_pointCount == 2 && step.blockSolve) {
const vcp1 = this.v_points[0]; // VelocityConstraintPoint
const vcp2 = this.v_points[1]; // VelocityConstraintPoint
const rn1A = matrix.crossVec2Vec2(vcp1.rA, this.v_normal);
const rn1B = matrix.crossVec2Vec2(vcp1.rB, this.v_normal);
const rn2A = matrix.crossVec2Vec2(vcp2.rA, this.v_normal);
const rn2B = matrix.crossVec2Vec2(vcp2.rB, this.v_normal);
const k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
const k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
const k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const k_maxConditionNumber = 1000.0;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
// K is safe to invert.
this.v_K.ex.setNum(k11, k12);
this.v_K.ey.setNum(k12, k22);
// this.v_normalMass.set(this.v_K.getInverse());
const a = this.v_K.ex.x;
const b = this.v_K.ey.x;
const c = this.v_K.ex.y;
const d = this.v_K.ey.y;
let det = a * d - b * c;
if (det !== 0.0) {
det = 1.0 / det;
}
this.v_normalMass.ex.x = det * d;
this.v_normalMass.ey.x = -det * b;
this.v_normalMass.ex.y = -det * c;
this.v_normalMass.ey.y = det * a;
} else {
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
this.v_pointCount = 1;
}
}
matrix.copyVec2(positionA.c, cA);
positionA.a = aA;
matrix.copyVec2(velocityA.v, vA);
velocityA.w = wA;
matrix.copyVec2(positionB.c, cB);
positionB.a = aB;
matrix.copyVec2(velocityB.v, vB);
velocityB.w = wB;
}
warmStartConstraint(step: TimeStep): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const velocityA = bodyA.c_velocity;
const velocityB = bodyB.c_velocity;
// const positionA = bodyA.c_position;
// const positionB = bodyB.c_position;
const mA = this.v_invMassA;
const iA = this.v_invIA;
const mB = this.v_invMassB;
const iB = this.v_invIB;
matrix.copyVec2(vA, velocityA.v);
let wA = velocityA.w;
matrix.copyVec2(vB, velocityB.v);
let wB = velocityB.w;
matrix.copyVec2(normal, this.v_normal);
matrix.crossVec2Num(tangent, normal, 1.0);
for (let j = 0; j < this.v_pointCount; ++j) {
const vcp = this.v_points[j]; // VelocityConstraintPoint
matrix.combine2Vec2(P, vcp.normalImpulse, normal, vcp.tangentImpulse, tangent);
wA -= iA * matrix.crossVec2Vec2(vcp.rA, P);
matrix.minusScaleVec2(vA, mA, P);
wB += iB * matrix.crossVec2Vec2(vcp.rB, P);
matrix.plusScaleVec2(vB, mB, P);
}
matrix.copyVec2(velocityA.v, vA);
velocityA.w = wA;
matrix.copyVec2(velocityB.v, vB);
velocityB.w = wB;
}
storeConstraintImpulses(step: TimeStep): void {
const manifold = this.m_manifold;
for (let j = 0; j < this.v_pointCount; ++j) {
manifold.points[j].normalImpulse = this.v_points[j].normalImpulse;
manifold.points[j].tangentImpulse = this.v_points[j].tangentImpulse;
}
}
solveVelocityConstraint(step: TimeStep): void {
const fixtureA = this.m_fixtureA;
const fixtureB = this.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
const velocityA = bodyA.c_velocity;
// const positionA = bodyA.c_position;
const velocityB = bodyB.c_velocity;
// const positionB = bodyB.c_position;
const mA = this.v_invMassA;
const iA = this.v_invIA;
const mB = this.v_invMassB;
const iB = this.v_invIB;
matrix.copyVec2(vA, velocityA.v);
let wA = velocityA.w;
matrix.copyVec2(vB, velocityB.v);
let wB = velocityB.w;
matrix.copyVec2(normal, this.v_normal);
matrix.crossVec2Num(tangent, normal, 1.0);
const friction = this.v_friction;
if (_ASSERT) console.assert(this.v_pointCount == 1 || this.v_pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (let j = 0; j < this.v_pointCount; ++j) {
const vcp = this.v_points[j]; // VelocityConstraintPoint
// Relative velocity at contact
matrix.zeroVec2(dv);
matrix.plusVec2(dv, vB);
matrix.plusVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB));
matrix.minusVec2(dv, vA);
matrix.minusVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA));
// Compute tangent force
const vt = matrix.dotVec2(dv, tangent) - this.v_tangentSpeed;
let lambda = vcp.tangentMass * -vt;
// Clamp the accumulated force
const maxFriction = friction * vcp.normalImpulse;
const newImpulse = clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.tangentImpulse;
vcp.tangentImpulse = newImpulse;
// Apply contact impulse
matrix.scaleVec2(P, lambda, tangent);
matrix.minusScaleVec2(vA, mA, P);
wA -= iA * matrix.crossVec2Vec2(vcp.rA, P);
matrix.plusScaleVec2(vB, mB, P);
wB += iB * matrix.crossVec2Vec2(vcp.rB, P);
}
// Solve normal constraints
if (this.v_pointCount == 1 || step.blockSolve == false) {
for (let i = 0; i < this.v_pointCount; ++i) {
const vcp = this.v_points[i]; // VelocityConstraintPoint
// Relative velocity at contact
matrix.zeroVec2(dv);
matrix.plusVec2(dv, vB);
matrix.plusVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB));
matrix.minusVec2(dv, vA);
matrix.minusVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA));
// Compute normal impulse
const vn = matrix.dotVec2(dv, normal);
let lambda = -vcp.normalMass * (vn - vcp.velocityBias);
// Clamp the accumulated impulse
const newImpulse = math_max(vcp.normalImpulse + lambda, 0.0);
lambda = newImpulse - vcp.normalImpulse;
vcp.normalImpulse = newImpulse;
// Apply contact impulse
matrix.scaleVec2(P, lambda, normal);
matrix.minusScaleVec2(vA, mA, P);
wA -= iA * matrix.crossVec2Vec2(vcp.rA, P);
matrix.plusScaleVec2(vB, mB, P);
wB += iB * matrix.crossVec2Vec2(vcp.rB, P);
}
} else {
// Block solver developed in collaboration with Dirk Gregorius (back in
// 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty).
// The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0.
// So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and
// vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the
// iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental
// impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental
// impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
const vcp1 = this.v_points[0]; // VelocityConstraintPoint
const vcp2 = this.v_points[1]; // VelocityConstraintPoint
matrix.setVec2(a, vcp1.normalImpulse, vcp2.normalImpulse);
if (_ASSERT) console.assert(a.x >= 0.0 && a.y >= 0.0);
// Relative velocity at contact
// let dv1 = Vec2.zero().add(vB).add(Vec2.crossNumVec2(wB, vcp1.rB)).sub(vA).sub(Vec2.crossNumVec2(wA, vcp1.rA));
matrix.zeroVec2(dv1);
matrix.plusVec2(dv1, vB);
matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB));
matrix.minusVec2(dv1, vA);
matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA));
// let dv2 = Vec2.zero().add(vB).add(Vec2.crossNumVec2(wB, vcp2.rB)).sub(vA).sub(Vec2.crossNumVec2(wA, vcp2.rA));
matrix.zeroVec2(dv2);
matrix.plusVec2(dv2, vB);
matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB));
matrix.minusVec2(dv2, vA);
matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA));
// Compute normal velocity
let vn1 = matrix.dotVec2(dv1, normal);
let vn2 = matrix.dotVec2(dv2, normal);
matrix.setVec2(b, vn1 - vcp1.velocityBias, vn2 - vcp2.velocityBias);
// Compute b'
// b.sub(Mat22.mulVec2(this.v_K, a));
b.x -= this.v_K.ex.x * a.x + this.v_K.ey.x * a.y;
b.y -= this.v_K.ex.y * a.x + this.v_K.ey.y * a.y;
const k_errorTol = 1e-3;
// NOT_USED(k_errorTol);
while (true) {
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
// const x = Mat22.mulVec2(this.v_normalMass, b).neg();
matrix.zeroVec2(x);
x.x = -(this.v_normalMass.ex.x * b.x + this.v_normalMass.ey.x * b.y);
x.y = -(this.v_normalMass.ex.y * b.x + this.v_normalMass.ey.y * b.y);
if (x.x >= 0.0 && x.y >= 0.0) {
// Get the incremental impulse
matrix.subVec2(d, x, a);
// Apply incremental impulse
matrix.scaleVec2(P1, d.x, normal);
matrix.scaleVec2(P2, d.y, normal);
// vA.subCombine(mA, P1, mA, P2);
matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA);
wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2));
// vB.addCombine(mB, P1, mB, P2);
matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB);
wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2));
// Accumulate
vcp1.normalImpulse = x.x;
vcp2.normalImpulse = x.y;
if (DEBUG_SOLVER) {
// Postconditions
matrix.zeroVec2(dv1);
matrix.plusVec2(dv1, vB);
matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB));
matrix.minusVec2(dv1, vA);
matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA));
matrix.zeroVec2(dv2);
matrix.plusVec2(dv2, vB);
matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB));
matrix.minusVec2(dv2, vA);
matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA));
// Compute normal velocity
vn1 = matrix.dotVec2(dv1, normal);
vn2 = matrix.dotVec2(dv2, normal);
if (_ASSERT) console.assert(math_abs(vn1 - vcp1.velocityBias) < k_errorTol);
if (_ASSERT) console.assert(math_abs(vn2 - vcp2.velocityBias) < k_errorTol);
}
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.x = -vcp1.normalMass * b.x;
x.y = 0.0;
vn1 = 0.0;
vn2 = this.v_K.ex.y * x.x + b.y;
if (x.x >= 0.0 && vn2 >= 0.0) {
// Get the incremental impulse
matrix.subVec2(d, x, a);
// Apply incremental impulse
matrix.scaleVec2(P1, d.x, normal);
matrix.scaleVec2(P2, d.y, normal);
// vA.subCombine(mA, P1, mA, P2);
matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA);
wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2));
// vB.addCombine(mB, P1, mB, P2);
matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB);
wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2));
// Accumulate
vcp1.normalImpulse = x.x;
vcp2.normalImpulse = x.y;
if (DEBUG_SOLVER) {
// Postconditions
matrix.zeroVec2(dv1);
matrix.plusVec2(dv1, vB);
matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB));
matrix.minusVec2(dv1, vA);
matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA));
// Compute normal velocity
vn1 = matrix.dotVec2(dv1, normal);
if (_ASSERT) console.assert(math_abs(vn1 - vcp1.velocityBias) < k_errorTol);
}
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.x = 0.0;
x.y = -vcp2.normalMass * b.y;
vn1 = this.v_K.ey.x * x.y + b.x;
vn2 = 0.0;
if (x.y >= 0.0 && vn1 >= 0.0) {
// Resubstitute for the incremental impulse
matrix.subVec2(d, x, a);
// Apply incremental impulse
matrix.scaleVec2(P1, d.x, normal);
matrix.scaleVec2(P2, d.y, normal);
// vA.subCombine(mA, P1, mA, P2);
matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA);
wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2));
// vB.addCombine(mB, P1, mB, P2);
matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB);
wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2));
// Accumulate
vcp1.normalImpulse = x.x;
vcp2.normalImpulse = x.y;
if (DEBUG_SOLVER) {
// Postconditions
matrix.zeroVec2(dv2);
matrix.plusVec2(dv2, vB);
matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB));
matrix.minusVec2(dv2, vA);
matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA));
// Compute normal velocity
vn2 = matrix.dotVec2(dv2, normal);
if (_ASSERT) console.assert(math_abs(vn2 - vcp2.velocityBias) < k_errorTol);
}
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
//
x.x = 0.0;
x.y = 0.0;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0 && vn2 >= 0.0) {
// Resubstitute for the incremental impulse
matrix.subVec2(d, x, a);
// Apply incremental impulse
matrix.scaleVec2(P1, d.x, normal);
matrix.scaleVec2(P2, d.y, normal);
// vA.subCombine(mA, P1, mA, P2);
matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA);
wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2));
// vB.addCombine(mB, P1, mB, P2);
matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB);
wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2));
// Accumulate
vcp1.normalImpulse = x.x;
vcp2.normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to
// matter.
break;
}
}
matrix.copyVec2(velocityA.v, vA);
velocityA.w = wA;
matrix.copyVec2(velocityB.v, vB);
velocityB.w = wB;
}
/** @internal */
static addType(type1: ShapeType, type2: ShapeType, callback: EvaluateFunction): void {
s_registers[type1] = s_registers[type1] || {};
s_registers[type1][type2] = callback;
}
/** @internal */
static create(fixtureA: Fixture, indexA: number, fixtureB: Fixture, indexB: number): Contact | null {
const typeA = fixtureA.m_shape.m_type;
const typeB = fixtureB.m_shape.m_type;
const contact = contactPool.allocate();
let evaluateFcn;
if ((evaluateFcn = s_registers[typeA] && s_registers[typeA][typeB])) {
contact.initialize(fixtureA, indexA, fixtureB, indexB, evaluateFcn);
} else if ((evaluateFcn = s_registers[typeB] && s_registers[typeB][typeA])) {
contact.initialize(fixtureB, indexB, fixtureA, indexA, evaluateFcn);
} else {
return null;
}
// Contact creation may swap fixtures.
fixtureA = contact.m_fixtureA;
fixtureB = contact.m_fixtureB;
indexA = contact.getChildIndexA();
indexB = contact.getChildIndexB();
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
// Connect to body A
contact.m_nodeA.contact = contact;
contact.m_nodeA.other = bodyB;
contact.m_nodeA.prev = null;
contact.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null) {
bodyA.m_contactList.prev = contact.m_nodeA;
}
bodyA.m_contactList = contact.m_nodeA;
// Connect to body B
contact.m_nodeB.contact = contact;
contact.m_nodeB.other = bodyA;
contact.m_nodeB.prev = null;
contact.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null) {
bodyB.m_contactList.prev = contact.m_nodeB;
}
bodyB.m_contactList = contact.m_nodeB;
// Wake up the bodies
if (fixtureA.isSensor() == false && fixtureB.isSensor() == false) {
bodyA.setAwake(true);
bodyB.setAwake(true);
}
return contact;
}
/** @internal */
static destroy(contact: Contact, listener: { endContact: (contact: Contact) => void }): void {
const fixtureA = contact.m_fixtureA;
const fixtureB = contact.m_fixtureB;
if (fixtureA === null || fixtureB === null) return;
const bodyA = fixtureA.m_body;
const bodyB = fixtureB.m_body;
if (bodyA === null || bodyB === null) return;
if (contact.isTouching()) {
listener.endContact(contact);
}
// Remove from body 1
if (contact.m_nodeA.prev) {
contact.m_nodeA.prev.next = contact.m_nodeA.next;
}
if (contact.m_nodeA.next) {
contact.m_nodeA.next.prev = contact.m_nodeA.prev;
}
if (contact.m_nodeA == bodyA.m_contactList) {
bodyA.m_contactList = contact.m_nodeA.next;
}
// Remove from body 2
if (contact.m_nodeB.prev) {
contact.m_nodeB.prev.next = contact.m_nodeB.next;
}
if (contact.m_nodeB.next) {
contact.m_nodeB.next.prev = contact.m_nodeB.prev;
}
if (contact.m_nodeB == bodyB.m_contactList) {
bodyB.m_contactList = contact.m_nodeB.next;
}
if (contact.m_manifold.pointCount > 0 && !fixtureA.m_isSensor && !fixtureB.m_isSensor) {
bodyA.setAwake(true);
bodyB.setAwake(true);
}
// const typeA = fixtureA.getType();
// const typeB = fixtureB.getType();
// const destroyFcn = s_registers[typeA][typeB].destroyFcn;
// if (typeof destroyFcn === 'function') {
// destroyFcn(contact);
// }
contactPool.release(contact);
}
}