14743 lines
596 KiB
JavaScript
14743 lines
596 KiB
JavaScript
/**
|
|
* Planck.js v1.0.0-alpha.2
|
|
* @license The MIT license
|
|
* @copyright Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
|
|
(function (global, factory) {
|
|
typeof exports === 'object' && typeof module !== 'undefined' ? factory(exports) :
|
|
typeof define === 'function' && define.amd ? define(['exports'], factory) :
|
|
(global = typeof globalThis !== 'undefined' ? globalThis : global || self, factory(global.planck = {}));
|
|
}(this, (function (exports) { 'use strict';
|
|
|
|
/*! *****************************************************************************
|
|
Copyright (c) Microsoft Corporation.
|
|
|
|
Permission to use, copy, modify, and/or distribute this software for any
|
|
purpose with or without fee is hereby granted.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
|
|
REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
|
|
AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
|
|
INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
|
|
LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
|
|
OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
|
|
PERFORMANCE OF THIS SOFTWARE.
|
|
***************************************************************************** */
|
|
/* global Reflect, Promise */
|
|
|
|
var extendStatics = function(d, b) {
|
|
extendStatics = Object.setPrototypeOf ||
|
|
({ __proto__: [] } instanceof Array && function (d, b) { d.__proto__ = b; }) ||
|
|
function (d, b) { for (var p in b) if (Object.prototype.hasOwnProperty.call(b, p)) d[p] = b[p]; };
|
|
return extendStatics(d, b);
|
|
};
|
|
|
|
function __extends(d, b) {
|
|
if (typeof b !== "function" && b !== null)
|
|
throw new TypeError("Class extends value " + String(b) + " is not a constructor or null");
|
|
extendStatics(d, b);
|
|
function __() { this.constructor = d; }
|
|
d.prototype = b === null ? Object.create(b) : (__.prototype = b.prototype, new __());
|
|
}
|
|
|
|
var __assign = function() {
|
|
__assign = Object.assign || function __assign(t) {
|
|
for (var s, i = 1, n = arguments.length; i < n; i++) {
|
|
s = arguments[i];
|
|
for (var p in s) if (Object.prototype.hasOwnProperty.call(s, p)) t[p] = s[p];
|
|
}
|
|
return t;
|
|
};
|
|
return __assign.apply(this, arguments);
|
|
};
|
|
|
|
function options (input, defaults) {
|
|
if (input === null || typeof input === 'undefined') {
|
|
input = {};
|
|
}
|
|
var output = __assign({}, input);
|
|
// tslint:disable-next-line:no-for-in
|
|
for (var key in defaults) {
|
|
if (defaults.hasOwnProperty(key) && typeof input[key] === 'undefined') {
|
|
output[key] = defaults[key];
|
|
}
|
|
}
|
|
if (typeof Object.getOwnPropertySymbols === 'function') {
|
|
var symbols = Object.getOwnPropertySymbols(defaults);
|
|
for (var i = 0; i < symbols.length; i++) {
|
|
var symbol = symbols[i];
|
|
if (defaults.propertyIsEnumerable(symbol) && typeof input[symbol] === 'undefined') {
|
|
output[symbol] = defaults[symbol];
|
|
}
|
|
}
|
|
}
|
|
return output;
|
|
}
|
|
|
|
var debug = function () {
|
|
var rest = [];
|
|
for (var _i = 0; _i < arguments.length; _i++) {
|
|
rest[_i] = arguments[_i];
|
|
}
|
|
return;
|
|
};
|
|
var assert = function (statement, err, log) {
|
|
return;
|
|
};
|
|
var common = {
|
|
assert: assert,
|
|
debug: debug,
|
|
};
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var math = Object.create(Math);
|
|
// @ts-ignore
|
|
// noinspection JSConstantReassignment
|
|
math.EPSILON = 1e-9; // TODO
|
|
math.isFinite = function (x) {
|
|
return (typeof x === 'number') && isFinite(x) && !isNaN(x);
|
|
};
|
|
math.assert = function (x) {
|
|
return;
|
|
};
|
|
math.invSqrt = function (x) {
|
|
// TODO:
|
|
return 1 / Math.sqrt(x);
|
|
};
|
|
math.nextPowerOfTwo = function (x) {
|
|
// TODO
|
|
x |= (x >> 1);
|
|
x |= (x >> 2);
|
|
x |= (x >> 4);
|
|
x |= (x >> 8);
|
|
x |= (x >> 16);
|
|
return x + 1;
|
|
};
|
|
math.isPowerOfTwo = function (x) {
|
|
return x > 0 && (x & (x - 1)) === 0;
|
|
};
|
|
math.mod = function (num, min, max) {
|
|
if (typeof min === 'undefined') {
|
|
max = 1;
|
|
min = 0;
|
|
}
|
|
else if (typeof max === 'undefined') {
|
|
max = min;
|
|
min = 0;
|
|
}
|
|
if (max > min) {
|
|
num = (num - min) % (max - min);
|
|
return num + (num < 0 ? max : min);
|
|
}
|
|
else {
|
|
num = (num - max) % (min - max);
|
|
return num + (num <= 0 ? min : max);
|
|
}
|
|
};
|
|
math.clamp = function (num, min, max) {
|
|
if (num < min) {
|
|
return min;
|
|
}
|
|
else if (num > max) {
|
|
return max;
|
|
}
|
|
else {
|
|
return num;
|
|
}
|
|
};
|
|
math.random = function (min, max) {
|
|
if (typeof min === 'undefined') {
|
|
max = 1;
|
|
min = 0;
|
|
}
|
|
else if (typeof max === 'undefined') {
|
|
max = min;
|
|
min = 0;
|
|
}
|
|
return min === max ? min : Math.random() * (max - min) + min;
|
|
};
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var Vec2 = /** @class */ (function () {
|
|
function Vec2(x, y) {
|
|
if (!(this instanceof Vec2)) {
|
|
return new Vec2(x, y);
|
|
}
|
|
if (typeof x === 'undefined') {
|
|
this.x = 0;
|
|
this.y = 0;
|
|
}
|
|
else if (typeof x === 'object') {
|
|
this.x = x.x;
|
|
this.y = x.y;
|
|
}
|
|
else {
|
|
this.x = x;
|
|
this.y = y;
|
|
}
|
|
}
|
|
/** @internal */
|
|
Vec2.prototype._serialize = function () {
|
|
return {
|
|
x: this.x,
|
|
y: this.y
|
|
};
|
|
};
|
|
/** @internal */
|
|
Vec2._deserialize = function (data) {
|
|
var obj = Object.create(Vec2.prototype);
|
|
obj.x = data.x;
|
|
obj.y = data.y;
|
|
return obj;
|
|
};
|
|
Vec2.zero = function () {
|
|
var obj = Object.create(Vec2.prototype);
|
|
obj.x = 0;
|
|
obj.y = 0;
|
|
return obj;
|
|
};
|
|
/** @internal */
|
|
Vec2.neo = function (x, y) {
|
|
var obj = Object.create(Vec2.prototype);
|
|
obj.x = x;
|
|
obj.y = y;
|
|
return obj;
|
|
};
|
|
Vec2.clone = function (v) {
|
|
return Vec2.neo(v.x, v.y);
|
|
};
|
|
Vec2.prototype.toString = function () {
|
|
return JSON.stringify(this);
|
|
};
|
|
/**
|
|
* Does this vector contain finite coordinates?
|
|
*/
|
|
Vec2.isValid = function (v) {
|
|
return v && math.isFinite(v.x) && math.isFinite(v.y);
|
|
};
|
|
Vec2.assert = function (o) {
|
|
return;
|
|
};
|
|
Vec2.prototype.clone = function () {
|
|
return Vec2.clone(this);
|
|
};
|
|
/**
|
|
* Set this vector to all zeros.
|
|
*
|
|
* @returns this
|
|
*/
|
|
Vec2.prototype.setZero = function () {
|
|
this.x = 0.0;
|
|
this.y = 0.0;
|
|
return this;
|
|
};
|
|
/**
|
|
* Set this vector to some specified coordinates.
|
|
*
|
|
* @returns this
|
|
*/
|
|
Vec2.prototype.set = function (x, y) {
|
|
if (typeof x === 'object') {
|
|
this.x = x.x;
|
|
this.y = x.y;
|
|
}
|
|
else {
|
|
this.x = x;
|
|
this.y = y;
|
|
}
|
|
return this;
|
|
};
|
|
/**
|
|
* @deprecated Use setCombine or setMul
|
|
*/
|
|
Vec2.prototype.wSet = function (a, v, b, w) {
|
|
if (typeof b !== 'undefined' || typeof w !== 'undefined') {
|
|
return this.setCombine(a, v, b, w);
|
|
}
|
|
else {
|
|
return this.setMul(a, v);
|
|
}
|
|
};
|
|
/**
|
|
* Set linear combination of v and w: `a * v + b * w`
|
|
*/
|
|
Vec2.prototype.setCombine = function (a, v, b, w) {
|
|
var x = a * v.x + b * w.x;
|
|
var y = a * v.y + b * w.y;
|
|
// `this` may be `w`
|
|
this.x = x;
|
|
this.y = y;
|
|
return this;
|
|
};
|
|
Vec2.prototype.setMul = function (a, v) {
|
|
var x = a * v.x;
|
|
var y = a * v.y;
|
|
this.x = x;
|
|
this.y = y;
|
|
return this;
|
|
};
|
|
/**
|
|
* Add a vector to this vector.
|
|
*
|
|
* @returns this
|
|
*/
|
|
Vec2.prototype.add = function (w) {
|
|
this.x += w.x;
|
|
this.y += w.y;
|
|
return this;
|
|
};
|
|
/**
|
|
* @deprecated Use addCombine or addMul
|
|
*/
|
|
Vec2.prototype.wAdd = function (a, v, b, w) {
|
|
if (typeof b !== 'undefined' || typeof w !== 'undefined') {
|
|
return this.addCombine(a, v, b, w);
|
|
}
|
|
else {
|
|
return this.addMul(a, v);
|
|
}
|
|
};
|
|
/**
|
|
* Add linear combination of v and w: `a * v + b * w`
|
|
*/
|
|
Vec2.prototype.addCombine = function (a, v, b, w) {
|
|
var x = a * v.x + b * w.x;
|
|
var y = a * v.y + b * w.y;
|
|
// `this` may be `w`
|
|
this.x += x;
|
|
this.y += y;
|
|
return this;
|
|
};
|
|
Vec2.prototype.addMul = function (a, v) {
|
|
var x = a * v.x;
|
|
var y = a * v.y;
|
|
this.x += x;
|
|
this.y += y;
|
|
return this;
|
|
};
|
|
/**
|
|
* @deprecated Use subCombine or subMul
|
|
*/
|
|
Vec2.prototype.wSub = function (a, v, b, w) {
|
|
if (typeof b !== 'undefined' || typeof w !== 'undefined') {
|
|
return this.subCombine(a, v, b, w);
|
|
}
|
|
else {
|
|
return this.subMul(a, v);
|
|
}
|
|
};
|
|
/**
|
|
* Subtract linear combination of v and w: `a * v + b * w`
|
|
*/
|
|
Vec2.prototype.subCombine = function (a, v, b, w) {
|
|
var x = a * v.x + b * w.x;
|
|
var y = a * v.y + b * w.y;
|
|
// `this` may be `w`
|
|
this.x -= x;
|
|
this.y -= y;
|
|
return this;
|
|
};
|
|
Vec2.prototype.subMul = function (a, v) {
|
|
var x = a * v.x;
|
|
var y = a * v.y;
|
|
this.x -= x;
|
|
this.y -= y;
|
|
return this;
|
|
};
|
|
/**
|
|
* Subtract a vector from this vector
|
|
*
|
|
* @returns this
|
|
*/
|
|
Vec2.prototype.sub = function (w) {
|
|
this.x -= w.x;
|
|
this.y -= w.y;
|
|
return this;
|
|
};
|
|
/**
|
|
* Multiply this vector by a scalar.
|
|
*
|
|
* @returns this
|
|
*/
|
|
Vec2.prototype.mul = function (m) {
|
|
this.x *= m;
|
|
this.y *= m;
|
|
return this;
|
|
};
|
|
/**
|
|
* Get the length of this vector (the norm).
|
|
*
|
|
* For performance, use this instead of lengthSquared (if possible).
|
|
*/
|
|
Vec2.prototype.length = function () {
|
|
return Vec2.lengthOf(this);
|
|
};
|
|
/**
|
|
* Get the length squared.
|
|
*/
|
|
Vec2.prototype.lengthSquared = function () {
|
|
return Vec2.lengthSquared(this);
|
|
};
|
|
/**
|
|
* Convert this vector into a unit vector.
|
|
*
|
|
* @returns old length
|
|
*/
|
|
Vec2.prototype.normalize = function () {
|
|
var length = this.length();
|
|
if (length < math.EPSILON) {
|
|
return 0.0;
|
|
}
|
|
var invLength = 1.0 / length;
|
|
this.x *= invLength;
|
|
this.y *= invLength;
|
|
return length;
|
|
};
|
|
/**
|
|
* Get the length of this vector (the norm).
|
|
*
|
|
* For performance, use this instead of lengthSquared (if possible).
|
|
*/
|
|
Vec2.lengthOf = function (v) {
|
|
return math.sqrt(v.x * v.x + v.y * v.y);
|
|
};
|
|
/**
|
|
* Get the length squared.
|
|
*/
|
|
Vec2.lengthSquared = function (v) {
|
|
return v.x * v.x + v.y * v.y;
|
|
};
|
|
Vec2.distance = function (v, w) {
|
|
var dx = v.x - w.x;
|
|
var dy = v.y - w.y;
|
|
return math.sqrt(dx * dx + dy * dy);
|
|
};
|
|
Vec2.distanceSquared = function (v, w) {
|
|
var dx = v.x - w.x;
|
|
var dy = v.y - w.y;
|
|
return dx * dx + dy * dy;
|
|
};
|
|
Vec2.areEqual = function (v, w) {
|
|
return v === w || typeof w === 'object' && w !== null && v.x === w.x && v.y === w.y;
|
|
};
|
|
/**
|
|
* Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
|
|
*/
|
|
Vec2.skew = function (v) {
|
|
return Vec2.neo(-v.y, v.x);
|
|
};
|
|
/**
|
|
* Perform the dot product on two vectors.
|
|
*/
|
|
Vec2.dot = function (v, w) {
|
|
return v.x * w.x + v.y * w.y;
|
|
};
|
|
/**
|
|
* Perform the cross product on two vectors. In 2D this produces a scalar.
|
|
*
|
|
* Perform the cross product on a vector and a scalar. In 2D this produces a
|
|
* vector.
|
|
*/
|
|
Vec2.cross = function (v, w) {
|
|
if (typeof w === 'number') {
|
|
return Vec2.neo(w * v.y, -w * v.x);
|
|
}
|
|
else if (typeof v === 'number') {
|
|
return Vec2.neo(-v * w.y, v * w.x);
|
|
}
|
|
else {
|
|
return v.x * w.y - v.y * w.x;
|
|
}
|
|
};
|
|
/**
|
|
* Returns `a + (v x w)`
|
|
*/
|
|
Vec2.addCross = function (a, v, w) {
|
|
if (typeof w === 'number') {
|
|
return Vec2.neo(w * v.y + a.x, -w * v.x + a.y);
|
|
}
|
|
else if (typeof v === 'number') {
|
|
return Vec2.neo(-v * w.y + a.x, v * w.x + a.y);
|
|
}
|
|
};
|
|
Vec2.add = function (v, w) {
|
|
return Vec2.neo(v.x + w.x, v.y + w.y);
|
|
};
|
|
/**
|
|
* @deprecated Use combine
|
|
*/
|
|
Vec2.wAdd = function (a, v, b, w) {
|
|
if (typeof b !== 'undefined' || typeof w !== 'undefined') {
|
|
return Vec2.combine(a, v, b, w);
|
|
}
|
|
else {
|
|
return Vec2.mul(a, v);
|
|
}
|
|
};
|
|
Vec2.combine = function (a, v, b, w) {
|
|
return Vec2.zero().setCombine(a, v, b, w);
|
|
};
|
|
Vec2.sub = function (v, w) {
|
|
return Vec2.neo(v.x - w.x, v.y - w.y);
|
|
};
|
|
Vec2.mul = function (a, b) {
|
|
if (typeof a === 'object') {
|
|
return Vec2.neo(a.x * b, a.y * b);
|
|
}
|
|
else if (typeof b === 'object') {
|
|
return Vec2.neo(a * b.x, a * b.y);
|
|
}
|
|
};
|
|
Vec2.prototype.neg = function () {
|
|
this.x = -this.x;
|
|
this.y = -this.y;
|
|
return this;
|
|
};
|
|
Vec2.neg = function (v) {
|
|
return Vec2.neo(-v.x, -v.y);
|
|
};
|
|
Vec2.abs = function (v) {
|
|
return Vec2.neo(math.abs(v.x), math.abs(v.y));
|
|
};
|
|
Vec2.mid = function (v, w) {
|
|
return Vec2.neo((v.x + w.x) * 0.5, (v.y + w.y) * 0.5);
|
|
};
|
|
Vec2.upper = function (v, w) {
|
|
return Vec2.neo(math.max(v.x, w.x), math.max(v.y, w.y));
|
|
};
|
|
Vec2.lower = function (v, w) {
|
|
return Vec2.neo(math.min(v.x, w.x), math.min(v.y, w.y));
|
|
};
|
|
Vec2.prototype.clamp = function (max) {
|
|
var lengthSqr = this.x * this.x + this.y * this.y;
|
|
if (lengthSqr > max * max) {
|
|
var invLength = math.invSqrt(lengthSqr);
|
|
this.x *= invLength * max;
|
|
this.y *= invLength * max;
|
|
}
|
|
return this;
|
|
};
|
|
Vec2.clamp = function (v, max) {
|
|
v = Vec2.neo(v.x, v.y);
|
|
v.clamp(max);
|
|
return v;
|
|
};
|
|
/**
|
|
* @deprecated
|
|
*/
|
|
Vec2.scaleFn = function (x, y) {
|
|
return function (v) {
|
|
return Vec2.neo(v.x * x, v.y * y);
|
|
};
|
|
};
|
|
/**
|
|
* @deprecated
|
|
*/
|
|
Vec2.translateFn = function (x, y) {
|
|
return function (v) {
|
|
return Vec2.neo(v.x + x, v.y + y);
|
|
};
|
|
};
|
|
return Vec2;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var AABB = /** @class */ (function () {
|
|
function AABB(lower, upper) {
|
|
if (!(this instanceof AABB)) {
|
|
return new AABB(lower, upper);
|
|
}
|
|
this.lowerBound = Vec2.zero();
|
|
this.upperBound = Vec2.zero();
|
|
if (typeof lower === 'object') {
|
|
this.lowerBound.set(lower);
|
|
}
|
|
if (typeof upper === 'object') {
|
|
this.upperBound.set(upper);
|
|
}
|
|
else if (typeof lower === 'object') {
|
|
this.upperBound.set(lower);
|
|
}
|
|
}
|
|
/**
|
|
* Verify that the bounds are sorted.
|
|
*/
|
|
AABB.prototype.isValid = function () {
|
|
return AABB.isValid(this);
|
|
};
|
|
AABB.isValid = function (aabb) {
|
|
var d = Vec2.sub(aabb.upperBound, aabb.lowerBound);
|
|
var valid = d.x >= 0.0 && d.y >= 0.0 && Vec2.isValid(aabb.lowerBound) && Vec2.isValid(aabb.upperBound);
|
|
return valid;
|
|
};
|
|
AABB.assert = function (o) {
|
|
return;
|
|
};
|
|
/**
|
|
* Get the center of the AABB.
|
|
*/
|
|
AABB.prototype.getCenter = function () {
|
|
return Vec2.neo((this.lowerBound.x + this.upperBound.x) * 0.5, (this.lowerBound.y + this.upperBound.y) * 0.5);
|
|
};
|
|
/**
|
|
* Get the extents of the AABB (half-widths).
|
|
*/
|
|
AABB.prototype.getExtents = function () {
|
|
return Vec2.neo((this.upperBound.x - this.lowerBound.x) * 0.5, (this.upperBound.y - this.lowerBound.y) * 0.5);
|
|
};
|
|
/**
|
|
* Get the perimeter length.
|
|
*/
|
|
AABB.prototype.getPerimeter = function () {
|
|
return 2.0 * (this.upperBound.x - this.lowerBound.x + this.upperBound.y - this.lowerBound.y);
|
|
};
|
|
/**
|
|
* Combine one or two AABB into this one.
|
|
*/
|
|
AABB.prototype.combine = function (a, b) {
|
|
b = b || this;
|
|
var lowerA = a.lowerBound;
|
|
var upperA = a.upperBound;
|
|
var lowerB = b.lowerBound;
|
|
var upperB = b.upperBound;
|
|
var lowerX = math.min(lowerA.x, lowerB.x);
|
|
var lowerY = math.min(lowerA.y, lowerB.y);
|
|
var upperX = math.max(upperB.x, upperA.x);
|
|
var upperY = math.max(upperB.y, upperA.y);
|
|
this.lowerBound.set(lowerX, lowerY);
|
|
this.upperBound.set(upperX, upperY);
|
|
};
|
|
AABB.prototype.combinePoints = function (a, b) {
|
|
this.lowerBound.set(math.min(a.x, b.x), math.min(a.y, b.y));
|
|
this.upperBound.set(math.max(a.x, b.x), math.max(a.y, b.y));
|
|
};
|
|
AABB.prototype.set = function (aabb) {
|
|
this.lowerBound.set(aabb.lowerBound.x, aabb.lowerBound.y);
|
|
this.upperBound.set(aabb.upperBound.x, aabb.upperBound.y);
|
|
};
|
|
AABB.prototype.contains = function (aabb) {
|
|
var result = true;
|
|
result = result && this.lowerBound.x <= aabb.lowerBound.x;
|
|
result = result && this.lowerBound.y <= aabb.lowerBound.y;
|
|
result = result && aabb.upperBound.x <= this.upperBound.x;
|
|
result = result && aabb.upperBound.y <= this.upperBound.y;
|
|
return result;
|
|
};
|
|
AABB.prototype.extend = function (value) {
|
|
AABB.extend(this, value);
|
|
return this;
|
|
};
|
|
AABB.extend = function (aabb, value) {
|
|
aabb.lowerBound.x -= value;
|
|
aabb.lowerBound.y -= value;
|
|
aabb.upperBound.x += value;
|
|
aabb.upperBound.y += value;
|
|
};
|
|
AABB.testOverlap = function (a, b) {
|
|
var d1x = b.lowerBound.x - a.upperBound.x;
|
|
var d2x = a.lowerBound.x - b.upperBound.x;
|
|
var d1y = b.lowerBound.y - a.upperBound.y;
|
|
var d2y = a.lowerBound.y - b.upperBound.y;
|
|
if (d1x > 0 || d1y > 0 || d2x > 0 || d2y > 0) {
|
|
return false;
|
|
}
|
|
return true;
|
|
};
|
|
AABB.areEqual = function (a, b) {
|
|
return Vec2.areEqual(a.lowerBound, b.lowerBound) && Vec2.areEqual(a.upperBound, b.upperBound);
|
|
};
|
|
AABB.diff = function (a, b) {
|
|
var wD = math.max(0, math.min(a.upperBound.x, b.upperBound.x) - math.max(b.lowerBound.x, a.lowerBound.x));
|
|
var hD = math.max(0, math.min(a.upperBound.y, b.upperBound.y) - math.max(b.lowerBound.y, a.lowerBound.y));
|
|
var wA = a.upperBound.x - a.lowerBound.x;
|
|
var hA = a.upperBound.y - a.lowerBound.y;
|
|
var wB = b.upperBound.x - b.lowerBound.x;
|
|
var hB = b.upperBound.y - b.lowerBound.y;
|
|
return wA * hA + wB * hB - wD * hD;
|
|
};
|
|
AABB.prototype.rayCast = function (output, input) {
|
|
// From Real-time Collision Detection, p179.
|
|
var tmin = -Infinity;
|
|
var tmax = Infinity;
|
|
var p = input.p1;
|
|
var d = Vec2.sub(input.p2, input.p1);
|
|
var absD = Vec2.abs(d);
|
|
var normal = Vec2.zero();
|
|
for (var f = 'x'; f !== null; f = (f === 'x' ? 'y' : null)) {
|
|
if (absD.x < math.EPSILON) {
|
|
// Parallel.
|
|
if (p[f] < this.lowerBound[f] || this.upperBound[f] < p[f]) {
|
|
return false;
|
|
}
|
|
}
|
|
else {
|
|
var inv_d = 1.0 / d[f];
|
|
var t1 = (this.lowerBound[f] - p[f]) * inv_d;
|
|
var t2 = (this.upperBound[f] - p[f]) * inv_d;
|
|
// Sign of the normal vector.
|
|
var s = -1.0;
|
|
if (t1 > t2) {
|
|
var temp = t1;
|
|
t1 = t2;
|
|
t2 = temp;
|
|
s = 1.0;
|
|
}
|
|
// Push the min up
|
|
if (t1 > tmin) {
|
|
normal.setZero();
|
|
normal[f] = s;
|
|
tmin = t1;
|
|
}
|
|
// Pull the max down
|
|
tmax = math.min(tmax, t2);
|
|
if (tmin > tmax) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
// Does the ray start inside the box?
|
|
// Does the ray intersect beyond the max fraction?
|
|
if (tmin < 0.0 || input.maxFraction < tmin) {
|
|
return false;
|
|
}
|
|
// Intersection.
|
|
output.fraction = tmin;
|
|
output.normal = normal;
|
|
return true;
|
|
};
|
|
AABB.prototype.toString = function () {
|
|
return JSON.stringify(this);
|
|
};
|
|
return AABB;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
// TODO merge with World options?
|
|
/**
|
|
* Tuning constants based on meters-kilograms-seconds (MKS) units.
|
|
*/
|
|
// tslint:disable-next-line:no-unnecessary-class
|
|
var Settings = /** @class */ (function () {
|
|
function Settings() {
|
|
}
|
|
Object.defineProperty(Settings, "linearSlopSquared", {
|
|
get: function () { return Settings.linearSlop * Settings.linearSlop; },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(Settings, "polygonRadius", {
|
|
/**
|
|
* The radius of the polygon/edge shape skin. This should not be modified.
|
|
* Making this smaller means polygons will have an insufficient buffer for
|
|
* continuous collision. Making it larger may create artifacts for vertex
|
|
* collision.
|
|
*/
|
|
get: function () { return 2.0 * Settings.linearSlop; },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(Settings, "maxTranslationSquared", {
|
|
get: function () { return Settings.maxTranslation * Settings.maxTranslation; },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(Settings, "maxRotationSquared", {
|
|
get: function () { return Settings.maxRotation * Settings.maxRotation; },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(Settings, "linearSleepToleranceSqr", {
|
|
get: function () { return Math.pow(Settings.linearSleepTolerance, 2); },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(Settings, "angularSleepToleranceSqr", {
|
|
get: function () { return Math.pow(Settings.angularSleepTolerance, 2); },
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
// Collision
|
|
/**
|
|
* The maximum number of contact points between two convex shapes. Do not change
|
|
* this value.
|
|
*/
|
|
Settings.maxManifoldPoints = 2;
|
|
/**
|
|
* The maximum number of vertices on a convex polygon. You cannot increase this
|
|
* too much because BlockAllocator has a maximum object size.
|
|
*/
|
|
Settings.maxPolygonVertices = 12;
|
|
/**
|
|
* This is used to fatten AABBs in the dynamic tree. This allows proxies to move
|
|
* by a small amount without triggering a tree adjustment. This is in meters.
|
|
*/
|
|
Settings.aabbExtension = 0.1;
|
|
/**
|
|
* This is used to fatten AABBs in the dynamic tree. This is used to predict the
|
|
* future position based on the current displacement. This is a dimensionless
|
|
* multiplier.
|
|
*/
|
|
Settings.aabbMultiplier = 2.0;
|
|
/**
|
|
* A small length used as a collision and constraint tolerance. Usually it is
|
|
* chosen to be numerically significant, but visually insignificant.
|
|
*/
|
|
Settings.linearSlop = 0.005;
|
|
/**
|
|
* A small angle used as a collision and constraint tolerance. Usually it is
|
|
* chosen to be numerically significant, but visually insignificant.
|
|
*/
|
|
Settings.angularSlop = (2.0 / 180.0 * Math.PI);
|
|
/**
|
|
* Maximum number of sub-steps per contact in continuous physics simulation.
|
|
*/
|
|
Settings.maxSubSteps = 8;
|
|
// Dynamics
|
|
/**
|
|
* Maximum number of contacts to be handled to solve a TOI impact.
|
|
*/
|
|
Settings.maxTOIContacts = 32;
|
|
/**
|
|
* Maximum iterations to solve a TOI.
|
|
*/
|
|
Settings.maxTOIIterations = 20;
|
|
/**
|
|
* Maximum iterations to find Distance.
|
|
*/
|
|
Settings.maxDistnceIterations = 20;
|
|
/**
|
|
* A velocity threshold for elastic collisions. Any collision with a relative
|
|
* linear velocity below this threshold will be treated as inelastic.
|
|
*/
|
|
Settings.velocityThreshold = 1.0;
|
|
/**
|
|
* The maximum linear position correction used when solving constraints. This
|
|
* helps to prevent overshoot.
|
|
*/
|
|
Settings.maxLinearCorrection = 0.2;
|
|
/**
|
|
* The maximum angular position correction used when solving constraints. This
|
|
* helps to prevent overshoot.
|
|
*/
|
|
Settings.maxAngularCorrection = (8.0 / 180.0 * Math.PI);
|
|
/**
|
|
* The maximum linear velocity of a body. This limit is very large and is used
|
|
* to prevent numerical problems. You shouldn't need to adjust Settings.
|
|
*/
|
|
Settings.maxTranslation = 2.0;
|
|
/**
|
|
* The maximum angular velocity of a body. This limit is very large and is used
|
|
* to prevent numerical problems. You shouldn't need to adjust Settings.
|
|
*/
|
|
Settings.maxRotation = (0.5 * Math.PI);
|
|
/**
|
|
* This scale factor controls how fast overlap is resolved. Ideally this would
|
|
* be 1 so that overlap is removed in one time step. However using values close
|
|
* to 1 often lead to overshoot.
|
|
*/
|
|
Settings.baumgarte = 0.2;
|
|
Settings.toiBaugarte = 0.75;
|
|
// Sleep
|
|
/**
|
|
* The time that a body must be still before it will go to sleep.
|
|
*/
|
|
Settings.timeToSleep = 0.5;
|
|
/**
|
|
* A body cannot sleep if its linear velocity is above this tolerance.
|
|
*/
|
|
Settings.linearSleepTolerance = 0.01;
|
|
/**
|
|
* A body cannot sleep if its angular velocity is above this tolerance.
|
|
*/
|
|
Settings.angularSleepTolerance = (2.0 / 180.0 * Math.PI);
|
|
return Settings;
|
|
}());
|
|
|
|
/*
|
|
* Copyright (c) 2016-2018 Ali Shakiba http://shakiba.me/planck.js
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
var Pool = /** @class */ (function () {
|
|
function Pool(opts) {
|
|
this._list = [];
|
|
this._max = Infinity;
|
|
this._createCount = 0;
|
|
this._outCount = 0;
|
|
this._inCount = 0;
|
|
this._discardCount = 0;
|
|
this._list = [];
|
|
this._max = opts.max || this._max;
|
|
this._createFn = opts.create;
|
|
this._outFn = opts.allocate;
|
|
this._inFn = opts.release;
|
|
this._discardFn = opts.discard;
|
|
}
|
|
Pool.prototype.max = function (n) {
|
|
if (typeof n === 'number') {
|
|
this._max = n;
|
|
return this;
|
|
}
|
|
return this._max;
|
|
};
|
|
Pool.prototype.size = function () {
|
|
return this._list.length;
|
|
};
|
|
Pool.prototype.allocate = function () {
|
|
var item;
|
|
if (this._list.length > 0) {
|
|
item = this._list.shift();
|
|
}
|
|
else {
|
|
this._createCount++;
|
|
if (typeof this._createFn === 'function') {
|
|
item = this._createFn();
|
|
}
|
|
else {
|
|
item = {};
|
|
}
|
|
}
|
|
this._outCount++;
|
|
if (typeof this._outFn === 'function') {
|
|
this._outFn(item);
|
|
}
|
|
return item;
|
|
};
|
|
Pool.prototype.release = function (item) {
|
|
if (this._list.length < this._max) {
|
|
this._inCount++;
|
|
if (typeof this._inFn === 'function') {
|
|
this._inFn(item);
|
|
}
|
|
this._list.push(item);
|
|
}
|
|
else {
|
|
this._discardCount++;
|
|
if (typeof this._discardFn === 'function') {
|
|
item = this._discardFn(item);
|
|
}
|
|
}
|
|
};
|
|
Pool.prototype.toString = function () {
|
|
return " +" + this._createCount + " >" + this._outCount + " <" + this._inCount + " -"
|
|
+ this._discardCount + " =" + this._list.length + "/" + this._max;
|
|
};
|
|
return Pool;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A node in the dynamic tree. The client does not interact with this directly.
|
|
*/
|
|
var TreeNode = /** @class */ (function () {
|
|
function TreeNode(id) {
|
|
/** Enlarged AABB */
|
|
this.aabb = new AABB();
|
|
this.userData = null;
|
|
this.parent = null;
|
|
this.child1 = null;
|
|
this.child2 = null;
|
|
/** 0: leaf, -1: free node */
|
|
this.height = -1;
|
|
this.id = id;
|
|
}
|
|
TreeNode.prototype.toString = function () {
|
|
return this.id + ": " + this.userData;
|
|
};
|
|
TreeNode.prototype.isLeaf = function () {
|
|
return this.child1 == null;
|
|
};
|
|
return TreeNode;
|
|
}());
|
|
/**
|
|
* A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. A
|
|
* dynamic tree arranges data in a binary tree to accelerate queries such as
|
|
* volume queries and ray casts. Leafs are proxies with an AABB. In the tree we
|
|
* expand the proxy AABB by `aabbExtension` so that the proxy AABB is bigger
|
|
* than the client object. This allows the client object to move by small
|
|
* amounts without triggering a tree update.
|
|
*
|
|
* Nodes are pooled and relocatable, so we use node indices rather than
|
|
* pointers.
|
|
*/
|
|
var DynamicTree = /** @class */ (function () {
|
|
function DynamicTree() {
|
|
this.inputPool = new Pool({
|
|
create: function () {
|
|
return {};
|
|
},
|
|
release: function (stack) {
|
|
}
|
|
});
|
|
this.stackPool = new Pool({
|
|
create: function () {
|
|
return [];
|
|
},
|
|
release: function (stack) {
|
|
stack.length = 0;
|
|
}
|
|
});
|
|
this.iteratorPool = new Pool({
|
|
create: function () {
|
|
return new Iterator();
|
|
},
|
|
release: function (iterator) {
|
|
iterator.close();
|
|
}
|
|
});
|
|
this.m_root = null;
|
|
this.m_nodes = {};
|
|
this.m_lastProxyId = 0;
|
|
this.m_pool = new Pool({
|
|
create: function () {
|
|
return new TreeNode();
|
|
}
|
|
});
|
|
}
|
|
/**
|
|
* Get proxy user data.
|
|
*
|
|
* @return the proxy user data or 0 if the id is invalid.
|
|
*/
|
|
DynamicTree.prototype.getUserData = function (id) {
|
|
var node = this.m_nodes[id];
|
|
return node.userData;
|
|
};
|
|
/**
|
|
* Get the fat AABB for a node id.
|
|
*
|
|
* @return the proxy user data or 0 if the id is invalid.
|
|
*/
|
|
DynamicTree.prototype.getFatAABB = function (id) {
|
|
var node = this.m_nodes[id];
|
|
return node.aabb;
|
|
};
|
|
DynamicTree.prototype.allocateNode = function () {
|
|
var node = this.m_pool.allocate();
|
|
node.id = ++this.m_lastProxyId;
|
|
node.userData = null;
|
|
node.parent = null;
|
|
node.child1 = null;
|
|
node.child2 = null;
|
|
node.height = -1;
|
|
this.m_nodes[node.id] = node;
|
|
return node;
|
|
};
|
|
DynamicTree.prototype.freeNode = function (node) {
|
|
this.m_pool.release(node);
|
|
node.height = -1;
|
|
// tslint:disable-next-line:no-dynamic-delete
|
|
delete this.m_nodes[node.id];
|
|
};
|
|
/**
|
|
* Create a proxy in the tree as a leaf node. We return the index of the node
|
|
* instead of a pointer so that we can grow the node pool.
|
|
*
|
|
* Create a proxy. Provide a tight fitting AABB and a userData pointer.
|
|
*/
|
|
DynamicTree.prototype.createProxy = function (aabb, userData) {
|
|
var node = this.allocateNode();
|
|
node.aabb.set(aabb);
|
|
// Fatten the aabb.
|
|
AABB.extend(node.aabb, Settings.aabbExtension);
|
|
node.userData = userData;
|
|
node.height = 0;
|
|
this.insertLeaf(node);
|
|
return node.id;
|
|
};
|
|
/**
|
|
* Destroy a proxy. This asserts if the id is invalid.
|
|
*/
|
|
DynamicTree.prototype.destroyProxy = function (id) {
|
|
var node = this.m_nodes[id];
|
|
this.removeLeaf(node);
|
|
this.freeNode(node);
|
|
};
|
|
/**
|
|
* Move a proxy with a swepted AABB. If the proxy has moved outside of its
|
|
* fattened AABB, then the proxy is removed from the tree and re-inserted.
|
|
* Otherwise the function returns immediately.
|
|
*
|
|
* @param d Displacement
|
|
*
|
|
* @return true if the proxy was re-inserted.
|
|
*/
|
|
DynamicTree.prototype.moveProxy = function (id, aabb, d) {
|
|
var node = this.m_nodes[id];
|
|
if (node.aabb.contains(aabb)) {
|
|
return false;
|
|
}
|
|
this.removeLeaf(node);
|
|
node.aabb.set(aabb);
|
|
// Extend AABB.
|
|
aabb = node.aabb;
|
|
AABB.extend(aabb, Settings.aabbExtension);
|
|
// Predict AABB displacement.
|
|
// const d = Vec2.mul(Settings.aabbMultiplier, displacement);
|
|
if (d.x < 0.0) {
|
|
aabb.lowerBound.x += d.x * Settings.aabbMultiplier;
|
|
}
|
|
else {
|
|
aabb.upperBound.x += d.x * Settings.aabbMultiplier;
|
|
}
|
|
if (d.y < 0.0) {
|
|
aabb.lowerBound.y += d.y * Settings.aabbMultiplier;
|
|
}
|
|
else {
|
|
aabb.upperBound.y += d.y * Settings.aabbMultiplier;
|
|
}
|
|
this.insertLeaf(node);
|
|
return true;
|
|
};
|
|
DynamicTree.prototype.insertLeaf = function (leaf) {
|
|
if (this.m_root == null) {
|
|
this.m_root = leaf;
|
|
this.m_root.parent = null;
|
|
return;
|
|
}
|
|
// Find the best sibling for this node
|
|
var leafAABB = leaf.aabb;
|
|
var index = this.m_root;
|
|
while (!index.isLeaf()) {
|
|
var child1 = index.child1;
|
|
var child2 = index.child2;
|
|
var area = index.aabb.getPerimeter();
|
|
var combinedAABB = new AABB();
|
|
combinedAABB.combine(index.aabb, leafAABB);
|
|
var combinedArea = combinedAABB.getPerimeter();
|
|
// Cost of creating a new parent for this node and the new leaf
|
|
var cost = 2.0 * combinedArea;
|
|
// Minimum cost of pushing the leaf further down the tree
|
|
var inheritanceCost = 2.0 * (combinedArea - area);
|
|
// Cost of descending into child1
|
|
var cost1 = void 0;
|
|
if (child1.isLeaf()) {
|
|
var aabb = new AABB();
|
|
aabb.combine(leafAABB, child1.aabb);
|
|
cost1 = aabb.getPerimeter() + inheritanceCost;
|
|
}
|
|
else {
|
|
var aabb = new AABB();
|
|
aabb.combine(leafAABB, child1.aabb);
|
|
var oldArea = child1.aabb.getPerimeter();
|
|
var newArea = aabb.getPerimeter();
|
|
cost1 = (newArea - oldArea) + inheritanceCost;
|
|
}
|
|
// Cost of descending into child2
|
|
var cost2 = void 0;
|
|
if (child2.isLeaf()) {
|
|
var aabb = new AABB();
|
|
aabb.combine(leafAABB, child2.aabb);
|
|
cost2 = aabb.getPerimeter() + inheritanceCost;
|
|
}
|
|
else {
|
|
var aabb = new AABB();
|
|
aabb.combine(leafAABB, child2.aabb);
|
|
var oldArea = child2.aabb.getPerimeter();
|
|
var newArea = aabb.getPerimeter();
|
|
cost2 = newArea - oldArea + inheritanceCost;
|
|
}
|
|
// Descend according to the minimum cost.
|
|
if (cost < cost1 && cost < cost2) {
|
|
break;
|
|
}
|
|
// Descend
|
|
if (cost1 < cost2) {
|
|
index = child1;
|
|
}
|
|
else {
|
|
index = child2;
|
|
}
|
|
}
|
|
var sibling = index;
|
|
// Create a new parent.
|
|
var oldParent = sibling.parent;
|
|
var newParent = this.allocateNode();
|
|
newParent.parent = oldParent;
|
|
newParent.userData = null;
|
|
newParent.aabb.combine(leafAABB, sibling.aabb);
|
|
newParent.height = sibling.height + 1;
|
|
if (oldParent != null) {
|
|
// The sibling was not the root.
|
|
if (oldParent.child1 === sibling) {
|
|
oldParent.child1 = newParent;
|
|
}
|
|
else {
|
|
oldParent.child2 = newParent;
|
|
}
|
|
newParent.child1 = sibling;
|
|
newParent.child2 = leaf;
|
|
sibling.parent = newParent;
|
|
leaf.parent = newParent;
|
|
}
|
|
else {
|
|
// The sibling was the root.
|
|
newParent.child1 = sibling;
|
|
newParent.child2 = leaf;
|
|
sibling.parent = newParent;
|
|
leaf.parent = newParent;
|
|
this.m_root = newParent;
|
|
}
|
|
// Walk back up the tree fixing heights and AABBs
|
|
index = leaf.parent;
|
|
while (index != null) {
|
|
index = this.balance(index);
|
|
var child1 = index.child1;
|
|
var child2 = index.child2;
|
|
index.height = 1 + math.max(child1.height, child2.height);
|
|
index.aabb.combine(child1.aabb, child2.aabb);
|
|
index = index.parent;
|
|
}
|
|
// validate();
|
|
};
|
|
DynamicTree.prototype.removeLeaf = function (leaf) {
|
|
if (leaf === this.m_root) {
|
|
this.m_root = null;
|
|
return;
|
|
}
|
|
var parent = leaf.parent;
|
|
var grandParent = parent.parent;
|
|
var sibling;
|
|
if (parent.child1 === leaf) {
|
|
sibling = parent.child2;
|
|
}
|
|
else {
|
|
sibling = parent.child1;
|
|
}
|
|
if (grandParent != null) {
|
|
// Destroy parent and connect sibling to grandParent.
|
|
if (grandParent.child1 === parent) {
|
|
grandParent.child1 = sibling;
|
|
}
|
|
else {
|
|
grandParent.child2 = sibling;
|
|
}
|
|
sibling.parent = grandParent;
|
|
this.freeNode(parent);
|
|
// Adjust ancestor bounds.
|
|
var index = grandParent;
|
|
while (index != null) {
|
|
index = this.balance(index);
|
|
var child1 = index.child1;
|
|
var child2 = index.child2;
|
|
index.aabb.combine(child1.aabb, child2.aabb);
|
|
index.height = 1 + math.max(child1.height, child2.height);
|
|
index = index.parent;
|
|
}
|
|
}
|
|
else {
|
|
this.m_root = sibling;
|
|
sibling.parent = null;
|
|
this.freeNode(parent);
|
|
}
|
|
// validate();
|
|
};
|
|
/**
|
|
* Perform a left or right rotation if node A is imbalanced. Returns the new
|
|
* root index.
|
|
*/
|
|
DynamicTree.prototype.balance = function (iA) {
|
|
var A = iA;
|
|
if (A.isLeaf() || A.height < 2) {
|
|
return iA;
|
|
}
|
|
var B = A.child1;
|
|
var C = A.child2;
|
|
var balance = C.height - B.height;
|
|
// Rotate C up
|
|
if (balance > 1) {
|
|
var F = C.child1;
|
|
var G = C.child2;
|
|
// Swap A and C
|
|
C.child1 = A;
|
|
C.parent = A.parent;
|
|
A.parent = C;
|
|
// A's old parent should point to C
|
|
if (C.parent != null) {
|
|
if (C.parent.child1 === iA) {
|
|
C.parent.child1 = C;
|
|
}
|
|
else {
|
|
C.parent.child2 = C;
|
|
}
|
|
}
|
|
else {
|
|
this.m_root = C;
|
|
}
|
|
// Rotate
|
|
if (F.height > G.height) {
|
|
C.child2 = F;
|
|
A.child2 = G;
|
|
G.parent = A;
|
|
A.aabb.combine(B.aabb, G.aabb);
|
|
C.aabb.combine(A.aabb, F.aabb);
|
|
A.height = 1 + math.max(B.height, G.height);
|
|
C.height = 1 + math.max(A.height, F.height);
|
|
}
|
|
else {
|
|
C.child2 = G;
|
|
A.child2 = F;
|
|
F.parent = A;
|
|
A.aabb.combine(B.aabb, F.aabb);
|
|
C.aabb.combine(A.aabb, G.aabb);
|
|
A.height = 1 + math.max(B.height, F.height);
|
|
C.height = 1 + math.max(A.height, G.height);
|
|
}
|
|
return C;
|
|
}
|
|
// Rotate B up
|
|
if (balance < -1) {
|
|
var D = B.child1;
|
|
var E = B.child2;
|
|
// Swap A and B
|
|
B.child1 = A;
|
|
B.parent = A.parent;
|
|
A.parent = B;
|
|
// A's old parent should point to B
|
|
if (B.parent != null) {
|
|
if (B.parent.child1 === A) {
|
|
B.parent.child1 = B;
|
|
}
|
|
else {
|
|
B.parent.child2 = B;
|
|
}
|
|
}
|
|
else {
|
|
this.m_root = B;
|
|
}
|
|
// Rotate
|
|
if (D.height > E.height) {
|
|
B.child2 = D;
|
|
A.child1 = E;
|
|
E.parent = A;
|
|
A.aabb.combine(C.aabb, E.aabb);
|
|
B.aabb.combine(A.aabb, D.aabb);
|
|
A.height = 1 + math.max(C.height, E.height);
|
|
B.height = 1 + math.max(A.height, D.height);
|
|
}
|
|
else {
|
|
B.child2 = E;
|
|
A.child1 = D;
|
|
D.parent = A;
|
|
A.aabb.combine(C.aabb, D.aabb);
|
|
B.aabb.combine(A.aabb, E.aabb);
|
|
A.height = 1 + math.max(C.height, D.height);
|
|
B.height = 1 + math.max(A.height, E.height);
|
|
}
|
|
return B;
|
|
}
|
|
return A;
|
|
};
|
|
/**
|
|
* Compute the height of the binary tree in O(N) time. Should not be called
|
|
* often.
|
|
*/
|
|
DynamicTree.prototype.getHeight = function () {
|
|
if (this.m_root == null) {
|
|
return 0;
|
|
}
|
|
return this.m_root.height;
|
|
};
|
|
/**
|
|
* Get the ratio of the sum of the node areas to the root area.
|
|
*/
|
|
DynamicTree.prototype.getAreaRatio = function () {
|
|
if (this.m_root == null) {
|
|
return 0.0;
|
|
}
|
|
var root = this.m_root;
|
|
var rootArea = root.aabb.getPerimeter();
|
|
var totalArea = 0.0;
|
|
var node;
|
|
var it = this.iteratorPool.allocate().preorder(this.m_root);
|
|
while (node = it.next()) {
|
|
if (node.height < 0) {
|
|
// Free node in pool
|
|
continue;
|
|
}
|
|
totalArea += node.aabb.getPerimeter();
|
|
}
|
|
this.iteratorPool.release(it);
|
|
return totalArea / rootArea;
|
|
};
|
|
/**
|
|
* Compute the height of a sub-tree.
|
|
*/
|
|
DynamicTree.prototype.computeHeight = function (id) {
|
|
var node;
|
|
if (typeof id !== 'undefined') {
|
|
node = this.m_nodes[id];
|
|
}
|
|
else {
|
|
node = this.m_root;
|
|
}
|
|
// _ASSERT && common.assert(0 <= id && id < this.m_nodeCapacity);
|
|
if (node.isLeaf()) {
|
|
return 0;
|
|
}
|
|
var height1 = this.computeHeight(node.child1.id);
|
|
var height2 = this.computeHeight(node.child2.id);
|
|
return 1 + math.max(height1, height2);
|
|
};
|
|
DynamicTree.prototype.validateStructure = function (node) {
|
|
if (node == null) {
|
|
return;
|
|
}
|
|
if (node === this.m_root) ;
|
|
var child1 = node.child1;
|
|
var child2 = node.child2;
|
|
if (node.isLeaf()) {
|
|
return;
|
|
}
|
|
this.validateStructure(child1);
|
|
this.validateStructure(child2);
|
|
};
|
|
DynamicTree.prototype.validateMetrics = function (node) {
|
|
if (node == null) {
|
|
return;
|
|
}
|
|
var child1 = node.child1;
|
|
var child2 = node.child2;
|
|
if (node.isLeaf()) {
|
|
return;
|
|
}
|
|
// _ASSERT && common.assert(0 <= child1 && child1 < this.m_nodeCapacity);
|
|
// _ASSERT && common.assert(0 <= child2 && child2 < this.m_nodeCapacity);
|
|
var height1 = child1.height;
|
|
var height2 = child2.height;
|
|
1 + math.max(height1, height2);
|
|
var aabb = new AABB();
|
|
aabb.combine(child1.aabb, child2.aabb);
|
|
this.validateMetrics(child1);
|
|
this.validateMetrics(child2);
|
|
};
|
|
/**
|
|
* Validate this tree. For testing.
|
|
*/
|
|
DynamicTree.prototype.validate = function () {
|
|
this.validateStructure(this.m_root);
|
|
this.validateMetrics(this.m_root);
|
|
};
|
|
/**
|
|
* Get the maximum balance of an node in the tree. The balance is the difference
|
|
* in height of the two children of a node.
|
|
*/
|
|
DynamicTree.prototype.getMaxBalance = function () {
|
|
var maxBalance = 0;
|
|
var node;
|
|
var it = this.iteratorPool.allocate().preorder(this.m_root);
|
|
while (node = it.next()) {
|
|
if (node.height <= 1) {
|
|
continue;
|
|
}
|
|
var balance = math.abs(node.child2.height - node.child1.height);
|
|
maxBalance = math.max(maxBalance, balance);
|
|
}
|
|
this.iteratorPool.release(it);
|
|
return maxBalance;
|
|
};
|
|
/**
|
|
* Build an optimal tree. Very expensive. For testing.
|
|
*/
|
|
DynamicTree.prototype.rebuildBottomUp = function () {
|
|
var nodes = [];
|
|
var count = 0;
|
|
// Build array of leaves. Free the rest.
|
|
var node;
|
|
var it = this.iteratorPool.allocate().preorder(this.m_root);
|
|
while (node = it.next()) {
|
|
if (node.height < 0) {
|
|
// free node in pool
|
|
continue;
|
|
}
|
|
if (node.isLeaf()) {
|
|
node.parent = null;
|
|
nodes[count] = node;
|
|
++count;
|
|
}
|
|
else {
|
|
this.freeNode(node);
|
|
}
|
|
}
|
|
this.iteratorPool.release(it);
|
|
while (count > 1) {
|
|
var minCost = Infinity;
|
|
var iMin = -1;
|
|
var jMin = -1;
|
|
for (var i = 0; i < count; ++i) {
|
|
var aabbi = nodes[i].aabb;
|
|
for (var j = i + 1; j < count; ++j) {
|
|
var aabbj = nodes[j].aabb;
|
|
var b = new AABB();
|
|
b.combine(aabbi, aabbj);
|
|
var cost = b.getPerimeter();
|
|
if (cost < minCost) {
|
|
iMin = i;
|
|
jMin = j;
|
|
minCost = cost;
|
|
}
|
|
}
|
|
}
|
|
var child1 = nodes[iMin];
|
|
var child2 = nodes[jMin];
|
|
var parent_1 = this.allocateNode();
|
|
parent_1.child1 = child1;
|
|
parent_1.child2 = child2;
|
|
parent_1.height = 1 + math.max(child1.height, child2.height);
|
|
parent_1.aabb.combine(child1.aabb, child2.aabb);
|
|
parent_1.parent = null;
|
|
child1.parent = parent_1;
|
|
child2.parent = parent_1;
|
|
nodes[jMin] = nodes[count - 1];
|
|
nodes[iMin] = parent_1;
|
|
--count;
|
|
}
|
|
this.m_root = nodes[0];
|
|
this.validate();
|
|
};
|
|
/**
|
|
* Shift the world origin. Useful for large worlds. The shift formula is:
|
|
* position -= newOrigin
|
|
*
|
|
* @param newOrigin The new origin with respect to the old origin
|
|
*/
|
|
DynamicTree.prototype.shiftOrigin = function (newOrigin) {
|
|
// Build array of leaves. Free the rest.
|
|
var node;
|
|
var it = this.iteratorPool.allocate().preorder(this.m_root);
|
|
while (node = it.next()) {
|
|
var aabb = node.aabb;
|
|
aabb.lowerBound.x -= newOrigin.x;
|
|
aabb.lowerBound.y -= newOrigin.y;
|
|
aabb.upperBound.x -= newOrigin.x;
|
|
aabb.upperBound.y -= newOrigin.y;
|
|
}
|
|
this.iteratorPool.release(it);
|
|
};
|
|
/**
|
|
* Query an AABB for overlapping proxies. The callback class is called for each
|
|
* proxy that overlaps the supplied AABB.
|
|
*/
|
|
DynamicTree.prototype.query = function (aabb, queryCallback) {
|
|
var stack = this.stackPool.allocate();
|
|
stack.push(this.m_root);
|
|
while (stack.length > 0) {
|
|
var node = stack.pop();
|
|
if (node == null) {
|
|
continue;
|
|
}
|
|
if (AABB.testOverlap(node.aabb, aabb)) {
|
|
if (node.isLeaf()) {
|
|
var proceed = queryCallback(node.id);
|
|
if (proceed === false) {
|
|
return;
|
|
}
|
|
}
|
|
else {
|
|
stack.push(node.child1);
|
|
stack.push(node.child2);
|
|
}
|
|
}
|
|
}
|
|
this.stackPool.release(stack);
|
|
};
|
|
/**
|
|
* Ray-cast against the proxies in the tree. This relies on the callback to
|
|
* perform a exact ray-cast in the case were the proxy contains a shape. The
|
|
* callback also performs the any collision filtering. This has performance
|
|
* roughly equal to k * log(n), where k is the number of collisions and n is the
|
|
* number of proxies in the tree.
|
|
*
|
|
* @param input The ray-cast input data. The ray extends from `p1` to `p1 + maxFraction * (p2 - p1)`.
|
|
* @param rayCastCallback A function that is called for each proxy that is hit by the ray.
|
|
*/
|
|
DynamicTree.prototype.rayCast = function (input, rayCastCallback) {
|
|
var p1 = input.p1;
|
|
var p2 = input.p2;
|
|
var r = Vec2.sub(p2, p1);
|
|
r.normalize();
|
|
// v is perpendicular to the segment.
|
|
var v = Vec2.cross(1.0, r);
|
|
var abs_v = Vec2.abs(v);
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
var maxFraction = input.maxFraction;
|
|
// Build a bounding box for the segment.
|
|
var segmentAABB = new AABB();
|
|
var t = Vec2.combine((1 - maxFraction), p1, maxFraction, p2);
|
|
segmentAABB.combinePoints(p1, t);
|
|
var stack = this.stackPool.allocate();
|
|
var subInput = this.inputPool.allocate();
|
|
stack.push(this.m_root);
|
|
while (stack.length > 0) {
|
|
var node = stack.pop();
|
|
if (node == null) {
|
|
continue;
|
|
}
|
|
if (AABB.testOverlap(node.aabb, segmentAABB) === false) {
|
|
continue;
|
|
}
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
var c = node.aabb.getCenter();
|
|
var h = node.aabb.getExtents();
|
|
var separation = math.abs(Vec2.dot(v, Vec2.sub(p1, c))) - Vec2.dot(abs_v, h);
|
|
if (separation > 0.0) {
|
|
continue;
|
|
}
|
|
if (node.isLeaf()) {
|
|
subInput.p1 = Vec2.clone(input.p1);
|
|
subInput.p2 = Vec2.clone(input.p2);
|
|
subInput.maxFraction = maxFraction;
|
|
var value = rayCastCallback(subInput, node.id);
|
|
if (value === 0.0) {
|
|
// The client has terminated the ray cast.
|
|
return;
|
|
}
|
|
if (value > 0.0) {
|
|
// update segment bounding box.
|
|
maxFraction = value;
|
|
t = Vec2.combine((1 - maxFraction), p1, maxFraction, p2);
|
|
segmentAABB.combinePoints(p1, t);
|
|
}
|
|
}
|
|
else {
|
|
stack.push(node.child1);
|
|
stack.push(node.child2);
|
|
}
|
|
}
|
|
this.stackPool.release(stack);
|
|
this.inputPool.release(subInput);
|
|
};
|
|
return DynamicTree;
|
|
}());
|
|
var Iterator = /** @class */ (function () {
|
|
function Iterator() {
|
|
this.parents = [];
|
|
this.states = [];
|
|
}
|
|
Iterator.prototype.preorder = function (root) {
|
|
this.parents.length = 0;
|
|
this.parents.push(root);
|
|
this.states.length = 0;
|
|
this.states.push(0);
|
|
return this;
|
|
};
|
|
Iterator.prototype.next = function () {
|
|
while (this.parents.length > 0) {
|
|
var i = this.parents.length - 1;
|
|
var node = this.parents[i];
|
|
if (this.states[i] === 0) {
|
|
this.states[i] = 1;
|
|
return node;
|
|
}
|
|
if (this.states[i] === 1) {
|
|
this.states[i] = 2;
|
|
if (node.child1) {
|
|
this.parents.push(node.child1);
|
|
this.states.push(1);
|
|
return node.child1;
|
|
}
|
|
}
|
|
if (this.states[i] === 2) {
|
|
this.states[i] = 3;
|
|
if (node.child2) {
|
|
this.parents.push(node.child2);
|
|
this.states.push(1);
|
|
return node.child2;
|
|
}
|
|
}
|
|
this.parents.pop();
|
|
this.states.pop();
|
|
}
|
|
};
|
|
Iterator.prototype.close = function () {
|
|
this.parents.length = 0;
|
|
};
|
|
return Iterator;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* The broad-phase wraps and extends a dynamic-tree to keep track of moved
|
|
* objects and query them on update.
|
|
*/
|
|
var BroadPhase = /** @class */ (function () {
|
|
function BroadPhase() {
|
|
var _this = this;
|
|
this.m_tree = new DynamicTree();
|
|
this.m_proxyCount = 0;
|
|
this.m_moveBuffer = [];
|
|
/**
|
|
* Query an AABB for overlapping proxies. The callback class is called for each
|
|
* proxy that overlaps the supplied AABB.
|
|
*/
|
|
this.query = function (aabb, queryCallback) {
|
|
_this.m_tree.query(aabb, queryCallback);
|
|
};
|
|
this.queryCallback = function (proxyId) {
|
|
// A proxy cannot form a pair with itself.
|
|
if (proxyId === _this.m_queryProxyId) {
|
|
return true;
|
|
}
|
|
var proxyIdA = math.min(proxyId, _this.m_queryProxyId);
|
|
var proxyIdB = math.max(proxyId, _this.m_queryProxyId);
|
|
// TODO: Skip any duplicate pairs.
|
|
var userDataA = _this.m_tree.getUserData(proxyIdA);
|
|
var userDataB = _this.m_tree.getUserData(proxyIdB);
|
|
// Send the pairs back to the client.
|
|
_this.m_callback(userDataA, userDataB);
|
|
return true;
|
|
};
|
|
}
|
|
/**
|
|
* Get user data from a proxy. Returns null if the id is invalid.
|
|
*/
|
|
BroadPhase.prototype.getUserData = function (proxyId) {
|
|
return this.m_tree.getUserData(proxyId);
|
|
};
|
|
/**
|
|
* Test overlap of fat AABBs.
|
|
*/
|
|
BroadPhase.prototype.testOverlap = function (proxyIdA, proxyIdB) {
|
|
var aabbA = this.m_tree.getFatAABB(proxyIdA);
|
|
var aabbB = this.m_tree.getFatAABB(proxyIdB);
|
|
return AABB.testOverlap(aabbA, aabbB);
|
|
};
|
|
/**
|
|
* Get the fat AABB for a proxy.
|
|
*/
|
|
BroadPhase.prototype.getFatAABB = function (proxyId) {
|
|
return this.m_tree.getFatAABB(proxyId);
|
|
};
|
|
/**
|
|
* Get the number of proxies.
|
|
*/
|
|
BroadPhase.prototype.getProxyCount = function () {
|
|
return this.m_proxyCount;
|
|
};
|
|
/**
|
|
* Get the height of the embedded tree.
|
|
*/
|
|
BroadPhase.prototype.getTreeHeight = function () {
|
|
return this.m_tree.getHeight();
|
|
};
|
|
/**
|
|
* Get the balance (integer) of the embedded tree.
|
|
*/
|
|
BroadPhase.prototype.getTreeBalance = function () {
|
|
return this.m_tree.getMaxBalance();
|
|
};
|
|
/**
|
|
* Get the quality metric of the embedded tree.
|
|
*/
|
|
BroadPhase.prototype.getTreeQuality = function () {
|
|
return this.m_tree.getAreaRatio();
|
|
};
|
|
/**
|
|
* Ray-cast against the proxies in the tree. This relies on the callback to
|
|
* perform a exact ray-cast in the case were the proxy contains a shape. The
|
|
* callback also performs the any collision filtering. This has performance
|
|
* roughly equal to k * log(n), where k is the number of collisions and n is the
|
|
* number of proxies in the tree.
|
|
*
|
|
* @param input The ray-cast input data. The ray extends from `p1` to `p1 + maxFraction * (p2 - p1)`.
|
|
* @param rayCastCallback A function that is called for each proxy that is hit by the ray.
|
|
*/
|
|
BroadPhase.prototype.rayCast = function (input, rayCastCallback) {
|
|
this.m_tree.rayCast(input, rayCastCallback);
|
|
};
|
|
/**
|
|
* Shift the world origin. Useful for large worlds. The shift formula is:
|
|
* position -= newOrigin
|
|
*
|
|
* @param newOrigin The new origin with respect to the old origin
|
|
*/
|
|
BroadPhase.prototype.shiftOrigin = function (newOrigin) {
|
|
this.m_tree.shiftOrigin(newOrigin);
|
|
};
|
|
/**
|
|
* Create a proxy with an initial AABB. Pairs are not reported until UpdatePairs
|
|
* is called.
|
|
*/
|
|
BroadPhase.prototype.createProxy = function (aabb, userData) {
|
|
var proxyId = this.m_tree.createProxy(aabb, userData);
|
|
this.m_proxyCount++;
|
|
this.bufferMove(proxyId);
|
|
return proxyId;
|
|
};
|
|
/**
|
|
* Destroy a proxy. It is up to the client to remove any pairs.
|
|
*/
|
|
BroadPhase.prototype.destroyProxy = function (proxyId) {
|
|
this.unbufferMove(proxyId);
|
|
this.m_proxyCount--;
|
|
this.m_tree.destroyProxy(proxyId);
|
|
};
|
|
/**
|
|
* Call moveProxy as many times as you like, then when you are done call
|
|
* UpdatePairs to finalized the proxy pairs (for your time step).
|
|
*/
|
|
BroadPhase.prototype.moveProxy = function (proxyId, aabb, displacement) {
|
|
var changed = this.m_tree.moveProxy(proxyId, aabb, displacement);
|
|
if (changed) {
|
|
this.bufferMove(proxyId);
|
|
}
|
|
};
|
|
/**
|
|
* Call to trigger a re-processing of it's pairs on the next call to
|
|
* UpdatePairs.
|
|
*/
|
|
BroadPhase.prototype.touchProxy = function (proxyId) {
|
|
this.bufferMove(proxyId);
|
|
};
|
|
BroadPhase.prototype.bufferMove = function (proxyId) {
|
|
this.m_moveBuffer.push(proxyId);
|
|
};
|
|
BroadPhase.prototype.unbufferMove = function (proxyId) {
|
|
for (var i = 0; i < this.m_moveBuffer.length; ++i) {
|
|
if (this.m_moveBuffer[i] === proxyId) {
|
|
this.m_moveBuffer[i] = null;
|
|
}
|
|
}
|
|
};
|
|
/**
|
|
* Update the pairs. This results in pair callbacks. This can only add pairs.
|
|
*/
|
|
BroadPhase.prototype.updatePairs = function (addPairCallback) {
|
|
this.m_callback = addPairCallback;
|
|
// Perform tree queries for all moving proxies.
|
|
while (this.m_moveBuffer.length > 0) {
|
|
this.m_queryProxyId = this.m_moveBuffer.pop();
|
|
if (this.m_queryProxyId === null) {
|
|
continue;
|
|
}
|
|
// We have to query the tree with the fat AABB so that
|
|
// we don't fail to create a pair that may touch later.
|
|
var fatAABB = this.m_tree.getFatAABB(this.m_queryProxyId);
|
|
// Query tree, create pairs and add them pair buffer.
|
|
this.m_tree.query(fatAABB, this.queryCallback);
|
|
}
|
|
// Try to keep the tree balanced.
|
|
// this.m_tree.rebalance(4);
|
|
};
|
|
return BroadPhase;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var Rot = /** @class */ (function () {
|
|
/** Initialize from an angle in radians. */
|
|
function Rot(angle) {
|
|
if (!(this instanceof Rot)) {
|
|
return new Rot(angle);
|
|
}
|
|
if (typeof angle === 'number') {
|
|
this.setAngle(angle);
|
|
}
|
|
else if (typeof angle === 'object') {
|
|
this.set(angle);
|
|
}
|
|
else {
|
|
this.setIdentity();
|
|
}
|
|
}
|
|
/** @internal */
|
|
Rot.neo = function (angle) {
|
|
var obj = Object.create(Rot.prototype);
|
|
obj.setAngle(angle);
|
|
return obj;
|
|
};
|
|
Rot.clone = function (rot) {
|
|
var obj = Object.create(Rot.prototype);
|
|
obj.s = rot.s;
|
|
obj.c = rot.c;
|
|
return obj;
|
|
};
|
|
Rot.identity = function () {
|
|
var obj = Object.create(Rot.prototype);
|
|
obj.s = 0.0;
|
|
obj.c = 1.0;
|
|
return obj;
|
|
};
|
|
Rot.isValid = function (o) {
|
|
return o && math.isFinite(o.s) && math.isFinite(o.c);
|
|
};
|
|
Rot.assert = function (o) {
|
|
return;
|
|
};
|
|
/** Set to the identity rotation. */
|
|
Rot.prototype.setIdentity = function () {
|
|
this.s = 0.0;
|
|
this.c = 1.0;
|
|
};
|
|
Rot.prototype.set = function (angle) {
|
|
if (typeof angle === 'object') {
|
|
this.s = angle.s;
|
|
this.c = angle.c;
|
|
}
|
|
else {
|
|
// TODO_ERIN optimize
|
|
this.s = math.sin(angle);
|
|
this.c = math.cos(angle);
|
|
}
|
|
};
|
|
/** Set using an angle in radians. */
|
|
Rot.prototype.setAngle = function (angle) {
|
|
// TODO_ERIN optimize
|
|
this.s = math.sin(angle);
|
|
this.c = math.cos(angle);
|
|
};
|
|
/** Get the angle in radians. */
|
|
Rot.prototype.getAngle = function () {
|
|
return math.atan2(this.s, this.c);
|
|
};
|
|
/** Get the x-axis. */
|
|
Rot.prototype.getXAxis = function () {
|
|
return Vec2.neo(this.c, this.s);
|
|
};
|
|
/** Get the u-axis. */
|
|
Rot.prototype.getYAxis = function () {
|
|
return Vec2.neo(-this.s, this.c);
|
|
};
|
|
Rot.mul = function (rot, m) {
|
|
if ('c' in m && 's' in m) {
|
|
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
|
|
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
|
|
// s = qs * rc + qc * rs
|
|
// c = qc * rc - qs * rs
|
|
var qr = Rot.identity();
|
|
qr.s = rot.s * m.c + rot.c * m.s;
|
|
qr.c = rot.c * m.c - rot.s * m.s;
|
|
return qr;
|
|
}
|
|
else if ('x' in m && 'y' in m) {
|
|
return Vec2.neo(rot.c * m.x - rot.s * m.y, rot.s * m.x + rot.c * m.y);
|
|
}
|
|
};
|
|
Rot.mulRot = function (rot, m) {
|
|
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
|
|
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
|
|
// s = qs * rc + qc * rs
|
|
// c = qc * rc - qs * rs
|
|
var qr = Rot.identity();
|
|
qr.s = rot.s * m.c + rot.c * m.s;
|
|
qr.c = rot.c * m.c - rot.s * m.s;
|
|
return qr;
|
|
};
|
|
Rot.mulVec2 = function (rot, m) {
|
|
return Vec2.neo(rot.c * m.x - rot.s * m.y, rot.s * m.x + rot.c * m.y);
|
|
};
|
|
Rot.mulSub = function (rot, v, w) {
|
|
var x = rot.c * (v.x - w.x) - rot.s * (v.y - w.y);
|
|
var y = rot.s * (v.x - w.x) + rot.c * (v.y - w.y);
|
|
return Vec2.neo(x, y);
|
|
};
|
|
Rot.mulT = function (rot, m) {
|
|
if ('c' in m && 's' in m) {
|
|
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
|
|
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
|
|
// s = qc * rs - qs * rc
|
|
// c = qc * rc + qs * rs
|
|
var qr = Rot.identity();
|
|
qr.s = rot.c * m.s - rot.s * m.c;
|
|
qr.c = rot.c * m.c + rot.s * m.s;
|
|
return qr;
|
|
}
|
|
else if ('x' in m && 'y' in m) {
|
|
return Vec2.neo(rot.c * m.x + rot.s * m.y, -rot.s * m.x + rot.c * m.y);
|
|
}
|
|
};
|
|
Rot.mulTRot = function (rot, m) {
|
|
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
|
|
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
|
|
// s = qc * rs - qs * rc
|
|
// c = qc * rc + qs * rs
|
|
var qr = Rot.identity();
|
|
qr.s = rot.c * m.s - rot.s * m.c;
|
|
qr.c = rot.c * m.c + rot.s * m.s;
|
|
return qr;
|
|
};
|
|
Rot.mulTVec2 = function (rot, m) {
|
|
return Vec2.neo(rot.c * m.x + rot.s * m.y, -rot.s * m.x + rot.c * m.y);
|
|
};
|
|
return Rot;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A transform contains translation and rotation. It is used to represent the
|
|
* position and orientation of rigid frames. Initialize using a position vector
|
|
* and a rotation.
|
|
*/
|
|
var Transform = /** @class */ (function () {
|
|
function Transform(position, rotation) {
|
|
if (!(this instanceof Transform)) {
|
|
return new Transform(position, rotation);
|
|
}
|
|
this.p = Vec2.zero();
|
|
this.q = Rot.identity();
|
|
if (typeof position !== 'undefined') {
|
|
this.p.set(position);
|
|
}
|
|
if (typeof rotation !== 'undefined') {
|
|
this.q.set(rotation);
|
|
}
|
|
}
|
|
Transform.clone = function (xf) {
|
|
var obj = Object.create(Transform.prototype);
|
|
obj.p = Vec2.clone(xf.p);
|
|
obj.q = Rot.clone(xf.q);
|
|
return obj;
|
|
};
|
|
/** @internal */
|
|
Transform.neo = function (position, rotation) {
|
|
var obj = Object.create(Transform.prototype);
|
|
obj.p = Vec2.clone(position);
|
|
obj.q = Rot.clone(rotation);
|
|
return obj;
|
|
};
|
|
Transform.identity = function () {
|
|
var obj = Object.create(Transform.prototype);
|
|
obj.p = Vec2.zero();
|
|
obj.q = Rot.identity();
|
|
return obj;
|
|
};
|
|
/**
|
|
* Set this to the identity transform.
|
|
*/
|
|
Transform.prototype.setIdentity = function () {
|
|
this.p.setZero();
|
|
this.q.setIdentity();
|
|
};
|
|
/**
|
|
* Set this based on the position and angle.
|
|
*/
|
|
Transform.prototype.set = function (a, b) {
|
|
if (typeof b === 'undefined') {
|
|
this.p.set(a.p);
|
|
this.q.set(a.q);
|
|
}
|
|
else {
|
|
this.p.set(a);
|
|
this.q.set(b);
|
|
}
|
|
};
|
|
Transform.isValid = function (o) {
|
|
return o && Vec2.isValid(o.p) && Rot.isValid(o.q);
|
|
};
|
|
Transform.assert = function (o) {
|
|
return;
|
|
};
|
|
// static mul(a: Transform, b: Vec2[]): Vec2[];
|
|
// static mul(a: Transform, b: Transform[]): Transform[];
|
|
Transform.mul = function (a, b) {
|
|
if (Array.isArray(b)) {
|
|
var arr = [];
|
|
for (var i = 0; i < b.length; i++) {
|
|
arr[i] = Transform.mul(a, b[i]);
|
|
}
|
|
return arr;
|
|
}
|
|
else if ('x' in b && 'y' in b) {
|
|
return Transform.mulVec2(a, b);
|
|
}
|
|
else if ('p' in b && 'q' in b) {
|
|
return Transform.mulXf(a, b);
|
|
}
|
|
};
|
|
Transform.mulAll = function (a, b) {
|
|
var arr = [];
|
|
for (var i = 0; i < b.length; i++) {
|
|
arr[i] = Transform.mul(a, b[i]);
|
|
}
|
|
return arr;
|
|
};
|
|
/** @deprecated */
|
|
Transform.mulFn = function (a) {
|
|
return function (b) {
|
|
return Transform.mul(a, b);
|
|
};
|
|
};
|
|
Transform.mulVec2 = function (a, b) {
|
|
var x = (a.q.c * b.x - a.q.s * b.y) + a.p.x;
|
|
var y = (a.q.s * b.x + a.q.c * b.y) + a.p.y;
|
|
return Vec2.neo(x, y);
|
|
};
|
|
Transform.mulXf = function (a, b) {
|
|
// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
|
|
// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
|
|
var xf = Transform.identity();
|
|
xf.q = Rot.mulRot(a.q, b.q);
|
|
xf.p = Vec2.add(Rot.mulVec2(a.q, b.p), a.p);
|
|
return xf;
|
|
};
|
|
Transform.mulT = function (a, b) {
|
|
if ('x' in b && 'y' in b) {
|
|
return Transform.mulTVec2(a, b);
|
|
}
|
|
else if ('p' in b && 'q' in b) {
|
|
return Transform.mulTXf(a, b);
|
|
}
|
|
};
|
|
Transform.mulTVec2 = function (a, b) {
|
|
var px = b.x - a.p.x;
|
|
var py = b.y - a.p.y;
|
|
var x = (a.q.c * px + a.q.s * py);
|
|
var y = (-a.q.s * px + a.q.c * py);
|
|
return Vec2.neo(x, y);
|
|
};
|
|
Transform.mulTXf = function (a, b) {
|
|
// v2 = A.q' * (B.q * v1 + B.p - A.p)
|
|
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
|
|
var xf = Transform.identity();
|
|
xf.q.set(Rot.mulTRot(a.q, b.q));
|
|
xf.p.set(Rot.mulTVec2(a.q, Vec2.sub(b.p, a.p)));
|
|
return xf;
|
|
};
|
|
return Transform;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* This describes the motion of a body/shape for TOI computation. Shapes are
|
|
* defined with respect to the body origin, which may not coincide with the
|
|
* center of mass. However, to support dynamics we must interpolate the center
|
|
* of mass position.
|
|
*/
|
|
var Sweep = /** @class */ (function () {
|
|
function Sweep(c, a) {
|
|
this.localCenter = Vec2.zero();
|
|
this.c = Vec2.zero();
|
|
this.a = 0;
|
|
this.alpha0 = 0;
|
|
this.c0 = Vec2.zero();
|
|
this.a0 = 0;
|
|
}
|
|
Sweep.prototype.setTransform = function (xf) {
|
|
var c = Transform.mulVec2(xf, this.localCenter);
|
|
this.c.set(c);
|
|
this.c0.set(c);
|
|
this.a = xf.q.getAngle();
|
|
this.a0 = xf.q.getAngle();
|
|
};
|
|
Sweep.prototype.setLocalCenter = function (localCenter, xf) {
|
|
this.localCenter.set(localCenter);
|
|
var c = Transform.mulVec2(xf, this.localCenter);
|
|
this.c.set(c);
|
|
this.c0.set(c);
|
|
};
|
|
/**
|
|
* Get the interpolated transform at a specific time.
|
|
*
|
|
* @param xf
|
|
* @param beta A factor in [0,1], where 0 indicates alpha0
|
|
*/
|
|
Sweep.prototype.getTransform = function (xf, beta) {
|
|
beta = typeof beta === 'undefined' ? 0 : beta;
|
|
xf.q.setAngle((1.0 - beta) * this.a0 + beta * this.a);
|
|
xf.p.setCombine((1.0 - beta), this.c0, beta, this.c);
|
|
// shift to origin
|
|
xf.p.sub(Rot.mulVec2(xf.q, this.localCenter));
|
|
};
|
|
/**
|
|
* Advance the sweep forward, yielding a new initial state.
|
|
*
|
|
* @param alpha The new initial time
|
|
*/
|
|
Sweep.prototype.advance = function (alpha) {
|
|
var beta = (alpha - this.alpha0) / (1.0 - this.alpha0);
|
|
this.c0.setCombine(beta, this.c, 1 - beta, this.c0);
|
|
this.a0 = beta * this.a + (1 - beta) * this.a0;
|
|
this.alpha0 = alpha;
|
|
};
|
|
Sweep.prototype.forward = function () {
|
|
this.a0 = this.a;
|
|
this.c0.set(this.c);
|
|
};
|
|
/**
|
|
* normalize the angles in radians to be between -pi and pi.
|
|
*/
|
|
Sweep.prototype.normalize = function () {
|
|
var a0 = math.mod(this.a0, -math.PI, +math.PI);
|
|
this.a -= this.a0 - a0;
|
|
this.a0 = a0;
|
|
};
|
|
Sweep.prototype.clone = function () {
|
|
var clone = new Sweep();
|
|
clone.localCenter.set(this.localCenter);
|
|
clone.alpha0 = this.alpha0;
|
|
clone.a0 = this.a0;
|
|
clone.a = this.a;
|
|
clone.c0.set(this.c0);
|
|
clone.c.set(this.c);
|
|
return clone;
|
|
};
|
|
Sweep.prototype.set = function (that) {
|
|
this.localCenter.set(that.localCenter);
|
|
this.alpha0 = that.alpha0;
|
|
this.a0 = that.a0;
|
|
this.a = that.a;
|
|
this.c0.set(that.c0);
|
|
this.c.set(that.c);
|
|
};
|
|
return Sweep;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var Velocity = /** @class */ (function () {
|
|
function Velocity() {
|
|
this.v = Vec2.zero();
|
|
this.w = 0;
|
|
}
|
|
return Velocity;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var Position = /** @class */ (function () {
|
|
function Position() {
|
|
this.c = Vec2.zero();
|
|
this.a = 0;
|
|
}
|
|
Position.prototype.getTransform = function (xf, p) {
|
|
xf.q.set(this.a);
|
|
xf.p.set(Vec2.sub(this.c, Rot.mulVec2(xf.q, p)));
|
|
return xf;
|
|
};
|
|
return Position;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A shape is used for collision detection. You can create a shape however you
|
|
* like. Shapes used for simulation in World are created automatically when a
|
|
* Fixture is created. Shapes may encapsulate one or more child shapes.
|
|
*/
|
|
var Shape = /** @class */ (function () {
|
|
function Shape() {
|
|
}
|
|
Shape.prototype._reset = function () {
|
|
};
|
|
/** @internal */
|
|
Shape._deserialize = function (data, context, restore) {
|
|
var clazz = Shape.TYPES[data.type];
|
|
return clazz && restore(clazz, data);
|
|
};
|
|
Shape.isValid = function (shape) {
|
|
return !!shape;
|
|
};
|
|
Shape.prototype.getRadius = function () {
|
|
return this.m_radius;
|
|
};
|
|
/**
|
|
* Get the type of this shape. You can use this to down cast to the concrete
|
|
* shape.
|
|
*
|
|
* @return the shape type.
|
|
*/
|
|
Shape.prototype.getType = function () {
|
|
return this.m_type;
|
|
};
|
|
Shape.TYPES = {};
|
|
return Shape;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var FixtureDefDefault = {
|
|
userData: null,
|
|
friction: 0.2,
|
|
restitution: 0.0,
|
|
density: 0.0,
|
|
isSensor: false,
|
|
filterGroupIndex: 0,
|
|
filterCategoryBits: 0x0001,
|
|
filterMaskBits: 0xFFFF
|
|
};
|
|
/**
|
|
* This proxy is used internally to connect shape children to the broad-phase.
|
|
*/
|
|
var FixtureProxy = /** @class */ (function () {
|
|
function FixtureProxy(fixture, childIndex) {
|
|
this.aabb = new AABB();
|
|
this.fixture = fixture;
|
|
this.childIndex = childIndex;
|
|
this.proxyId;
|
|
}
|
|
return FixtureProxy;
|
|
}());
|
|
/**
|
|
* A fixture is used to attach a shape to a body for collision detection. A
|
|
* fixture inherits its transform from its parent. Fixtures hold additional
|
|
* non-geometric data such as friction, collision filters, etc. Fixtures are
|
|
* created via Body.createFixture.
|
|
*/
|
|
var Fixture = /** @class */ (function () {
|
|
function Fixture(body, shape, def) {
|
|
if (shape.shape) {
|
|
def = shape;
|
|
shape = shape.shape;
|
|
}
|
|
else if (typeof def === 'number') {
|
|
def = { density: def };
|
|
}
|
|
def = options(def, FixtureDefDefault);
|
|
this.m_body = body;
|
|
this.m_friction = def.friction;
|
|
this.m_restitution = def.restitution;
|
|
this.m_density = def.density;
|
|
this.m_isSensor = def.isSensor;
|
|
this.m_filterGroupIndex = def.filterGroupIndex;
|
|
this.m_filterCategoryBits = def.filterCategoryBits;
|
|
this.m_filterMaskBits = def.filterMaskBits;
|
|
// TODO validate shape
|
|
this.m_shape = shape; // .clone();
|
|
this.m_next = null;
|
|
this.m_proxies = [];
|
|
this.m_proxyCount = 0;
|
|
var childCount = this.m_shape.getChildCount();
|
|
for (var i = 0; i < childCount; ++i) {
|
|
this.m_proxies[i] = new FixtureProxy(this, i);
|
|
}
|
|
this.m_userData = def.userData;
|
|
}
|
|
/**
|
|
* Re-setup fixture.
|
|
* @private
|
|
*/
|
|
Fixture.prototype._reset = function () {
|
|
var body = this.getBody();
|
|
var broadPhase = body.m_world.m_broadPhase;
|
|
this.destroyProxies(broadPhase);
|
|
if (this.m_shape._reset) {
|
|
this.m_shape._reset();
|
|
}
|
|
var childCount = this.m_shape.getChildCount();
|
|
for (var i = 0; i < childCount; ++i) {
|
|
this.m_proxies[i] = new FixtureProxy(this, i);
|
|
}
|
|
this.createProxies(broadPhase, body.m_xf);
|
|
body.resetMassData();
|
|
};
|
|
/** @internal */
|
|
Fixture.prototype._serialize = function () {
|
|
return {
|
|
friction: this.m_friction,
|
|
restitution: this.m_restitution,
|
|
density: this.m_density,
|
|
isSensor: this.m_isSensor,
|
|
filterGroupIndex: this.m_filterGroupIndex,
|
|
filterCategoryBits: this.m_filterCategoryBits,
|
|
filterMaskBits: this.m_filterMaskBits,
|
|
shape: this.m_shape,
|
|
};
|
|
};
|
|
/** @internal */
|
|
Fixture._deserialize = function (data, body, restore) {
|
|
var shape = restore(Shape, data.shape);
|
|
var fixture = shape && new Fixture(body, shape, data);
|
|
return fixture;
|
|
};
|
|
/**
|
|
* Get the type of the child shape. You can use this to down cast to the
|
|
* concrete shape.
|
|
*/
|
|
Fixture.prototype.getType = function () {
|
|
return this.m_shape.getType();
|
|
};
|
|
/**
|
|
* Get the child shape. You can modify the child shape, however you should not
|
|
* change the number of vertices because this will crash some collision caching
|
|
* mechanisms. Manipulating the shape may lead to non-physical behavior.
|
|
*/
|
|
Fixture.prototype.getShape = function () {
|
|
return this.m_shape;
|
|
};
|
|
/**
|
|
* A sensor shape collects contact information but never generates a collision
|
|
* response.
|
|
*/
|
|
Fixture.prototype.isSensor = function () {
|
|
return this.m_isSensor;
|
|
};
|
|
/**
|
|
* Set if this fixture is a sensor.
|
|
*/
|
|
Fixture.prototype.setSensor = function (sensor) {
|
|
if (sensor != this.m_isSensor) {
|
|
this.m_body.setAwake(true);
|
|
this.m_isSensor = sensor;
|
|
}
|
|
};
|
|
/**
|
|
* Get the contact filtering data.
|
|
*/
|
|
// getFilterData() {
|
|
// return this.m_filter;
|
|
// }
|
|
/**
|
|
* Get the user data that was assigned in the fixture definition. Use this to
|
|
* store your application specific data.
|
|
*/
|
|
Fixture.prototype.getUserData = function () {
|
|
return this.m_userData;
|
|
};
|
|
/**
|
|
* Set the user data. Use this to store your application specific data.
|
|
*/
|
|
Fixture.prototype.setUserData = function (data) {
|
|
this.m_userData = data;
|
|
};
|
|
/**
|
|
* Get the parent body of this fixture. This is null if the fixture is not
|
|
* attached.
|
|
*/
|
|
Fixture.prototype.getBody = function () {
|
|
return this.m_body;
|
|
};
|
|
/**
|
|
* Get the next fixture in the parent body's fixture list.
|
|
*/
|
|
Fixture.prototype.getNext = function () {
|
|
return this.m_next;
|
|
};
|
|
/**
|
|
* Get the density of this fixture.
|
|
*/
|
|
Fixture.prototype.getDensity = function () {
|
|
return this.m_density;
|
|
};
|
|
/**
|
|
* Set the density of this fixture. This will _not_ automatically adjust the
|
|
* mass of the body. You must call Body.resetMassData to update the body's mass.
|
|
*/
|
|
Fixture.prototype.setDensity = function (density) {
|
|
this.m_density = density;
|
|
};
|
|
/**
|
|
* Get the coefficient of friction, usually in the range [0,1].
|
|
*/
|
|
Fixture.prototype.getFriction = function () {
|
|
return this.m_friction;
|
|
};
|
|
/**
|
|
* Set the coefficient of friction. This will not change the friction of
|
|
* existing contacts.
|
|
*/
|
|
Fixture.prototype.setFriction = function (friction) {
|
|
this.m_friction = friction;
|
|
};
|
|
/**
|
|
* Get the coefficient of restitution.
|
|
*/
|
|
Fixture.prototype.getRestitution = function () {
|
|
return this.m_restitution;
|
|
};
|
|
/**
|
|
* Set the coefficient of restitution. This will not change the restitution of
|
|
* existing contacts.
|
|
*/
|
|
Fixture.prototype.setRestitution = function (restitution) {
|
|
this.m_restitution = restitution;
|
|
};
|
|
/**
|
|
* Test a point in world coordinates for containment in this fixture.
|
|
*/
|
|
Fixture.prototype.testPoint = function (p) {
|
|
return this.m_shape.testPoint(this.m_body.getTransform(), p);
|
|
};
|
|
/**
|
|
* Cast a ray against this shape.
|
|
*/
|
|
Fixture.prototype.rayCast = function (output, input, childIndex) {
|
|
return this.m_shape.rayCast(output, input, this.m_body.getTransform(), childIndex);
|
|
};
|
|
/**
|
|
* Get the mass data for this fixture. The mass data is based on the density and
|
|
* the shape. The rotational inertia is about the shape's origin. This operation
|
|
* may be expensive.
|
|
*/
|
|
Fixture.prototype.getMassData = function (massData) {
|
|
this.m_shape.computeMass(massData, this.m_density);
|
|
};
|
|
/**
|
|
* Get the fixture's AABB. This AABB may be enlarge and/or stale. If you need a
|
|
* more accurate AABB, compute it using the shape and the body transform.
|
|
*/
|
|
Fixture.prototype.getAABB = function (childIndex) {
|
|
return this.m_proxies[childIndex].aabb;
|
|
};
|
|
/**
|
|
* These support body activation/deactivation.
|
|
*/
|
|
Fixture.prototype.createProxies = function (broadPhase, xf) {
|
|
// Create proxies in the broad-phase.
|
|
this.m_proxyCount = this.m_shape.getChildCount();
|
|
for (var i = 0; i < this.m_proxyCount; ++i) {
|
|
var proxy = this.m_proxies[i];
|
|
this.m_shape.computeAABB(proxy.aabb, xf, i);
|
|
proxy.proxyId = broadPhase.createProxy(proxy.aabb, proxy);
|
|
}
|
|
};
|
|
Fixture.prototype.destroyProxies = function (broadPhase) {
|
|
// Destroy proxies in the broad-phase.
|
|
for (var i = 0; i < this.m_proxyCount; ++i) {
|
|
var proxy = this.m_proxies[i];
|
|
broadPhase.destroyProxy(proxy.proxyId);
|
|
proxy.proxyId = null;
|
|
}
|
|
this.m_proxyCount = 0;
|
|
};
|
|
/**
|
|
* Updates this fixture proxy in broad-phase (with combined AABB of current and
|
|
* next transformation).
|
|
*/
|
|
Fixture.prototype.synchronize = function (broadPhase, xf1, xf2) {
|
|
for (var i = 0; i < this.m_proxyCount; ++i) {
|
|
var proxy = this.m_proxies[i];
|
|
// Compute an AABB that covers the swept shape (may miss some rotation
|
|
// effect).
|
|
var aabb1 = new AABB();
|
|
var aabb2 = new AABB();
|
|
this.m_shape.computeAABB(aabb1, xf1, proxy.childIndex);
|
|
this.m_shape.computeAABB(aabb2, xf2, proxy.childIndex);
|
|
proxy.aabb.combine(aabb1, aabb2);
|
|
var displacement = Vec2.sub(xf2.p, xf1.p);
|
|
broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement);
|
|
}
|
|
};
|
|
/**
|
|
* Set the contact filtering data. This will not update contacts until the next
|
|
* time step when either parent body is active and awake. This automatically
|
|
* calls refilter.
|
|
*/
|
|
Fixture.prototype.setFilterData = function (filter) {
|
|
this.m_filterGroupIndex = filter.groupIndex;
|
|
this.m_filterCategoryBits = filter.categoryBits;
|
|
this.m_filterMaskBits = filter.maskBits;
|
|
this.refilter();
|
|
};
|
|
Fixture.prototype.getFilterGroupIndex = function () {
|
|
return this.m_filterGroupIndex;
|
|
};
|
|
Fixture.prototype.setFilterGroupIndex = function (groupIndex) {
|
|
return this.m_filterGroupIndex = groupIndex;
|
|
};
|
|
Fixture.prototype.getFilterCategoryBits = function () {
|
|
return this.m_filterCategoryBits;
|
|
};
|
|
Fixture.prototype.setFilterCategoryBits = function (categoryBits) {
|
|
this.m_filterCategoryBits = categoryBits;
|
|
};
|
|
Fixture.prototype.getFilterMaskBits = function () {
|
|
return this.m_filterMaskBits;
|
|
};
|
|
Fixture.prototype.setFilterMaskBits = function (maskBits) {
|
|
this.m_filterMaskBits = maskBits;
|
|
};
|
|
/**
|
|
* Call this if you want to establish collision that was previously disabled by
|
|
* ContactFilter.
|
|
*/
|
|
Fixture.prototype.refilter = function () {
|
|
if (this.m_body == null) {
|
|
return;
|
|
}
|
|
// Flag associated contacts for filtering.
|
|
var edge = this.m_body.getContactList();
|
|
while (edge) {
|
|
var contact = edge.contact;
|
|
var fixtureA = contact.getFixtureA();
|
|
var fixtureB = contact.getFixtureB();
|
|
if (fixtureA == this || fixtureB == this) {
|
|
contact.flagForFiltering();
|
|
}
|
|
edge = edge.next;
|
|
}
|
|
var world = this.m_body.getWorld();
|
|
if (world == null) {
|
|
return;
|
|
}
|
|
// Touch each proxy so that new pairs may be created
|
|
var broadPhase = world.m_broadPhase;
|
|
for (var i = 0; i < this.m_proxyCount; ++i) {
|
|
broadPhase.touchProxy(this.m_proxies[i].proxyId);
|
|
}
|
|
};
|
|
/**
|
|
* Implement this method to provide collision filtering, if you want finer
|
|
* control over contact creation.
|
|
*
|
|
* Return true if contact calculations should be performed between these two
|
|
* fixtures.
|
|
*
|
|
* Warning: for performance reasons this is only called when the AABBs begin to
|
|
* overlap.
|
|
*/
|
|
Fixture.prototype.shouldCollide = function (that) {
|
|
if (that.m_filterGroupIndex === this.m_filterGroupIndex && that.m_filterGroupIndex !== 0) {
|
|
return that.m_filterGroupIndex > 0;
|
|
}
|
|
var collideA = (that.m_filterMaskBits & this.m_filterCategoryBits) !== 0;
|
|
var collideB = (that.m_filterCategoryBits & this.m_filterMaskBits) !== 0;
|
|
var collide = collideA && collideB;
|
|
return collide;
|
|
};
|
|
return Fixture;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var STATIC = 'static';
|
|
var KINEMATIC = 'kinematic';
|
|
var DYNAMIC = 'dynamic';
|
|
var BodyDefDefault = {
|
|
type: STATIC,
|
|
position: Vec2.zero(),
|
|
angle: 0.0,
|
|
linearVelocity: Vec2.zero(),
|
|
angularVelocity: 0.0,
|
|
linearDamping: 0.0,
|
|
angularDamping: 0.0,
|
|
fixedRotation: false,
|
|
bullet: false,
|
|
gravityScale: 1.0,
|
|
allowSleep: true,
|
|
awake: true,
|
|
active: true,
|
|
userData: null
|
|
};
|
|
/**
|
|
* MassData This holds the mass data computed for a shape.
|
|
*/
|
|
var MassData = /** @class */ (function () {
|
|
function MassData() {
|
|
/** The mass of the shape, usually in kilograms. */
|
|
this.mass = 0;
|
|
/** The position of the shape's centroid relative to the shape's origin. */
|
|
this.center = Vec2.zero();
|
|
/** The rotational inertia of the shape about the local origin. */
|
|
this.I = 0;
|
|
}
|
|
return MassData;
|
|
}());
|
|
/**
|
|
* A rigid body composed of one or more fixtures.
|
|
*/
|
|
var Body = /** @class */ (function () {
|
|
function Body(world, def) {
|
|
def = options(def, BodyDefDefault);
|
|
this.m_world = world;
|
|
this.m_awakeFlag = def.awake;
|
|
this.m_autoSleepFlag = def.allowSleep;
|
|
this.m_bulletFlag = def.bullet;
|
|
this.m_fixedRotationFlag = def.fixedRotation;
|
|
this.m_activeFlag = def.active;
|
|
this.m_islandFlag = false;
|
|
this.m_toiFlag = false;
|
|
this.m_userData = def.userData;
|
|
this.m_type = def.type;
|
|
if (this.m_type == DYNAMIC) {
|
|
this.m_mass = 1.0;
|
|
this.m_invMass = 1.0;
|
|
}
|
|
else {
|
|
this.m_mass = 0.0;
|
|
this.m_invMass = 0.0;
|
|
}
|
|
// Rotational inertia about the center of mass.
|
|
this.m_I = 0.0;
|
|
this.m_invI = 0.0;
|
|
// the body origin transform
|
|
this.m_xf = Transform.identity();
|
|
this.m_xf.p = Vec2.clone(def.position);
|
|
this.m_xf.q.setAngle(def.angle);
|
|
// the swept motion for CCD
|
|
this.m_sweep = new Sweep();
|
|
this.m_sweep.setTransform(this.m_xf);
|
|
// position and velocity correction
|
|
this.c_velocity = new Velocity();
|
|
this.c_position = new Position();
|
|
this.m_force = Vec2.zero();
|
|
this.m_torque = 0.0;
|
|
this.m_linearVelocity = Vec2.clone(def.linearVelocity);
|
|
this.m_angularVelocity = def.angularVelocity;
|
|
this.m_linearDamping = def.linearDamping;
|
|
this.m_angularDamping = def.angularDamping;
|
|
this.m_gravityScale = def.gravityScale;
|
|
this.m_sleepTime = 0.0;
|
|
this.m_jointList = null;
|
|
this.m_contactList = null;
|
|
this.m_fixtureList = null;
|
|
this.m_prev = null;
|
|
this.m_next = null;
|
|
this.m_destroyed = false;
|
|
}
|
|
/** @internal */
|
|
Body.prototype._serialize = function () {
|
|
var fixtures = [];
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
fixtures.push(f);
|
|
}
|
|
return {
|
|
type: this.m_type,
|
|
bullet: this.m_bulletFlag,
|
|
position: this.m_xf.p,
|
|
angle: this.m_xf.q.getAngle(),
|
|
linearVelocity: this.m_linearVelocity,
|
|
angularVelocity: this.m_angularVelocity,
|
|
fixtures: fixtures,
|
|
};
|
|
};
|
|
/** @internal */
|
|
Body._deserialize = function (data, world, restore) {
|
|
var body = new Body(world, data);
|
|
if (data.fixtures) {
|
|
for (var i = data.fixtures.length - 1; i >= 0; i--) {
|
|
var fixture = restore(Fixture, data.fixtures[i], body);
|
|
body._addFixture(fixture);
|
|
}
|
|
}
|
|
return body;
|
|
};
|
|
Body.prototype.isWorldLocked = function () {
|
|
return this.m_world && this.m_world.isLocked() ? true : false;
|
|
};
|
|
Body.prototype.getWorld = function () {
|
|
return this.m_world;
|
|
};
|
|
Body.prototype.getNext = function () {
|
|
return this.m_next;
|
|
};
|
|
Body.prototype.setUserData = function (data) {
|
|
this.m_userData = data;
|
|
};
|
|
Body.prototype.getUserData = function () {
|
|
return this.m_userData;
|
|
};
|
|
Body.prototype.getFixtureList = function () {
|
|
return this.m_fixtureList;
|
|
};
|
|
Body.prototype.getJointList = function () {
|
|
return this.m_jointList;
|
|
};
|
|
/**
|
|
* Warning: this list changes during the time step and you may miss some
|
|
* collisions if you don't use ContactListener.
|
|
*/
|
|
Body.prototype.getContactList = function () {
|
|
return this.m_contactList;
|
|
};
|
|
Body.prototype.isStatic = function () {
|
|
return this.m_type == STATIC;
|
|
};
|
|
Body.prototype.isDynamic = function () {
|
|
return this.m_type == DYNAMIC;
|
|
};
|
|
Body.prototype.isKinematic = function () {
|
|
return this.m_type == KINEMATIC;
|
|
};
|
|
/**
|
|
* This will alter the mass and velocity.
|
|
*/
|
|
Body.prototype.setStatic = function () {
|
|
this.setType(STATIC);
|
|
return this;
|
|
};
|
|
Body.prototype.setDynamic = function () {
|
|
this.setType(DYNAMIC);
|
|
return this;
|
|
};
|
|
Body.prototype.setKinematic = function () {
|
|
this.setType(KINEMATIC);
|
|
return this;
|
|
};
|
|
/**
|
|
* @internal
|
|
*/
|
|
Body.prototype.getType = function () {
|
|
return this.m_type;
|
|
};
|
|
/**
|
|
* @internal
|
|
*/
|
|
Body.prototype.setType = function (type) {
|
|
if (this.isWorldLocked() == true) {
|
|
return;
|
|
}
|
|
if (this.m_type == type) {
|
|
return;
|
|
}
|
|
this.m_type = type;
|
|
this.resetMassData();
|
|
if (this.m_type == STATIC) {
|
|
this.m_linearVelocity.setZero();
|
|
this.m_angularVelocity = 0.0;
|
|
this.m_sweep.forward();
|
|
this.synchronizeFixtures();
|
|
}
|
|
this.setAwake(true);
|
|
this.m_force.setZero();
|
|
this.m_torque = 0.0;
|
|
// Delete the attached contacts.
|
|
var ce = this.m_contactList;
|
|
while (ce) {
|
|
var ce0 = ce;
|
|
ce = ce.next;
|
|
this.m_world.destroyContact(ce0.contact);
|
|
}
|
|
this.m_contactList = null;
|
|
// Touch the proxies so that new contacts will be created (when appropriate)
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
var proxyCount = f.m_proxyCount;
|
|
for (var i = 0; i < proxyCount; ++i) {
|
|
broadPhase.touchProxy(f.m_proxies[i].proxyId);
|
|
}
|
|
}
|
|
};
|
|
Body.prototype.isBullet = function () {
|
|
return this.m_bulletFlag;
|
|
};
|
|
/**
|
|
* Should this body be treated like a bullet for continuous collision detection?
|
|
*/
|
|
Body.prototype.setBullet = function (flag) {
|
|
this.m_bulletFlag = !!flag;
|
|
};
|
|
Body.prototype.isSleepingAllowed = function () {
|
|
return this.m_autoSleepFlag;
|
|
};
|
|
Body.prototype.setSleepingAllowed = function (flag) {
|
|
this.m_autoSleepFlag = !!flag;
|
|
if (this.m_autoSleepFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
};
|
|
Body.prototype.isAwake = function () {
|
|
return this.m_awakeFlag;
|
|
};
|
|
/**
|
|
* Set the sleep state of the body. A sleeping body has very low CPU cost.
|
|
*
|
|
* @param flag Set to true to wake the body, false to put it to sleep.
|
|
*/
|
|
Body.prototype.setAwake = function (flag) {
|
|
if (flag) {
|
|
if (this.m_awakeFlag == false) {
|
|
this.m_awakeFlag = true;
|
|
this.m_sleepTime = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
this.m_awakeFlag = false;
|
|
this.m_sleepTime = 0.0;
|
|
this.m_linearVelocity.setZero();
|
|
this.m_angularVelocity = 0.0;
|
|
this.m_force.setZero();
|
|
this.m_torque = 0.0;
|
|
}
|
|
};
|
|
Body.prototype.isActive = function () {
|
|
return this.m_activeFlag;
|
|
};
|
|
/**
|
|
* Set the active state of the body. An inactive body is not simulated and
|
|
* cannot be collided with or woken up. If you pass a flag of true, all fixtures
|
|
* will be added to the broad-phase. If you pass a flag of false, all fixtures
|
|
* will be removed from the broad-phase and all contacts will be destroyed.
|
|
* Fixtures and joints are otherwise unaffected.
|
|
*
|
|
* You may continue to create/destroy fixtures and joints on inactive bodies.
|
|
* Fixtures on an inactive body are implicitly inactive and will not participate
|
|
* in collisions, ray-casts, or queries. Joints connected to an inactive body
|
|
* are implicitly inactive. An inactive body is still owned by a World object
|
|
* and remains
|
|
*/
|
|
Body.prototype.setActive = function (flag) {
|
|
if (flag == this.m_activeFlag) {
|
|
return;
|
|
}
|
|
this.m_activeFlag = !!flag;
|
|
if (this.m_activeFlag) {
|
|
// Create all proxies.
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
f.createProxies(broadPhase, this.m_xf);
|
|
}
|
|
// Contacts are created the next time step.
|
|
}
|
|
else {
|
|
// Destroy all proxies.
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
f.destroyProxies(broadPhase);
|
|
}
|
|
// Destroy the attached contacts.
|
|
var ce = this.m_contactList;
|
|
while (ce) {
|
|
var ce0 = ce;
|
|
ce = ce.next;
|
|
this.m_world.destroyContact(ce0.contact);
|
|
}
|
|
this.m_contactList = null;
|
|
}
|
|
};
|
|
Body.prototype.isFixedRotation = function () {
|
|
return this.m_fixedRotationFlag;
|
|
};
|
|
/**
|
|
* Set this body to have fixed rotation. This causes the mass to be reset.
|
|
*/
|
|
Body.prototype.setFixedRotation = function (flag) {
|
|
if (this.m_fixedRotationFlag == flag) {
|
|
return;
|
|
}
|
|
this.m_fixedRotationFlag = !!flag;
|
|
this.m_angularVelocity = 0.0;
|
|
this.resetMassData();
|
|
};
|
|
/**
|
|
* Get the world transform for the body's origin.
|
|
*/
|
|
Body.prototype.getTransform = function () {
|
|
return this.m_xf;
|
|
};
|
|
/**
|
|
* Set the position of the body's origin and rotation. Manipulating a body's
|
|
* transform may cause non-physical behavior. Note: contacts are updated on the
|
|
* next call to World.step.
|
|
*
|
|
* @param position The world position of the body's local origin.
|
|
* @param angle The world rotation in radians.
|
|
*/
|
|
Body.prototype.setTransform = function (position, angle) {
|
|
if (this.isWorldLocked() == true) {
|
|
return;
|
|
}
|
|
this.m_xf.set(position, angle);
|
|
this.m_sweep.setTransform(this.m_xf);
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
f.synchronize(broadPhase, this.m_xf, this.m_xf);
|
|
}
|
|
};
|
|
Body.prototype.synchronizeTransform = function () {
|
|
this.m_sweep.getTransform(this.m_xf, 1);
|
|
};
|
|
/**
|
|
* Update fixtures in broad-phase.
|
|
*/
|
|
Body.prototype.synchronizeFixtures = function () {
|
|
var xf = Transform.identity();
|
|
this.m_sweep.getTransform(xf, 0);
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
f.synchronize(broadPhase, xf, this.m_xf);
|
|
}
|
|
};
|
|
/**
|
|
* Used in TOI.
|
|
*/
|
|
Body.prototype.advance = function (alpha) {
|
|
// Advance to the new safe time. This doesn't sync the broad-phase.
|
|
this.m_sweep.advance(alpha);
|
|
this.m_sweep.c.set(this.m_sweep.c0);
|
|
this.m_sweep.a = this.m_sweep.a0;
|
|
this.m_sweep.getTransform(this.m_xf, 1);
|
|
};
|
|
/**
|
|
* Get the world position for the body's origin.
|
|
*/
|
|
Body.prototype.getPosition = function () {
|
|
return this.m_xf.p;
|
|
};
|
|
Body.prototype.setPosition = function (p) {
|
|
this.setTransform(p, this.m_sweep.a);
|
|
};
|
|
/**
|
|
* Get the current world rotation angle in radians.
|
|
*/
|
|
Body.prototype.getAngle = function () {
|
|
return this.m_sweep.a;
|
|
};
|
|
Body.prototype.setAngle = function (angle) {
|
|
this.setTransform(this.m_xf.p, angle);
|
|
};
|
|
/**
|
|
* Get the world position of the center of mass.
|
|
*/
|
|
Body.prototype.getWorldCenter = function () {
|
|
return this.m_sweep.c;
|
|
};
|
|
/**
|
|
* Get the local position of the center of mass.
|
|
*/
|
|
Body.prototype.getLocalCenter = function () {
|
|
return this.m_sweep.localCenter;
|
|
};
|
|
/**
|
|
* Get the linear velocity of the center of mass.
|
|
*
|
|
* @return the linear velocity of the center of mass.
|
|
*/
|
|
Body.prototype.getLinearVelocity = function () {
|
|
return this.m_linearVelocity;
|
|
};
|
|
/**
|
|
* Get the world linear velocity of a world point attached to this body.
|
|
*
|
|
* @param worldPoint A point in world coordinates.
|
|
*/
|
|
Body.prototype.getLinearVelocityFromWorldPoint = function (worldPoint) {
|
|
var localCenter = Vec2.sub(worldPoint, this.m_sweep.c);
|
|
return Vec2.add(this.m_linearVelocity, Vec2.cross(this.m_angularVelocity, localCenter));
|
|
};
|
|
/**
|
|
* Get the world velocity of a local point.
|
|
*
|
|
* @param localPoint A point in local coordinates.
|
|
*/
|
|
Body.prototype.getLinearVelocityFromLocalPoint = function (localPoint) {
|
|
return this.getLinearVelocityFromWorldPoint(this.getWorldPoint(localPoint));
|
|
};
|
|
/**
|
|
* Set the linear velocity of the center of mass.
|
|
*
|
|
* @param v The new linear velocity of the center of mass.
|
|
*/
|
|
Body.prototype.setLinearVelocity = function (v) {
|
|
if (this.m_type == STATIC) {
|
|
return;
|
|
}
|
|
if (Vec2.dot(v, v) > 0.0) {
|
|
this.setAwake(true);
|
|
}
|
|
this.m_linearVelocity.set(v);
|
|
};
|
|
/**
|
|
* Get the angular velocity.
|
|
*
|
|
* @returns the angular velocity in radians/second.
|
|
*/
|
|
Body.prototype.getAngularVelocity = function () {
|
|
return this.m_angularVelocity;
|
|
};
|
|
/**
|
|
* Set the angular velocity.
|
|
*
|
|
* @param omega The new angular velocity in radians/second.
|
|
*/
|
|
Body.prototype.setAngularVelocity = function (w) {
|
|
if (this.m_type == STATIC) {
|
|
return;
|
|
}
|
|
if (w * w > 0.0) {
|
|
this.setAwake(true);
|
|
}
|
|
this.m_angularVelocity = w;
|
|
};
|
|
Body.prototype.getLinearDamping = function () {
|
|
return this.m_linearDamping;
|
|
};
|
|
Body.prototype.setLinearDamping = function (linearDamping) {
|
|
this.m_linearDamping = linearDamping;
|
|
};
|
|
Body.prototype.getAngularDamping = function () {
|
|
return this.m_angularDamping;
|
|
};
|
|
Body.prototype.setAngularDamping = function (angularDamping) {
|
|
this.m_angularDamping = angularDamping;
|
|
};
|
|
Body.prototype.getGravityScale = function () {
|
|
return this.m_gravityScale;
|
|
};
|
|
/**
|
|
* Scale the gravity applied to this body.
|
|
*/
|
|
Body.prototype.setGravityScale = function (scale) {
|
|
this.m_gravityScale = scale;
|
|
};
|
|
/**
|
|
* Get the total mass of the body.
|
|
*
|
|
* @returns The mass, usually in kilograms (kg).
|
|
*/
|
|
Body.prototype.getMass = function () {
|
|
return this.m_mass;
|
|
};
|
|
/**
|
|
* Get the rotational inertia of the body about the local origin.
|
|
*
|
|
* @return the rotational inertia, usually in kg-m^2.
|
|
*/
|
|
Body.prototype.getInertia = function () {
|
|
return this.m_I + this.m_mass
|
|
* Vec2.dot(this.m_sweep.localCenter, this.m_sweep.localCenter);
|
|
};
|
|
/**
|
|
* Copy the mass data of the body to data.
|
|
*/
|
|
Body.prototype.getMassData = function (data) {
|
|
data.mass = this.m_mass;
|
|
data.I = this.getInertia();
|
|
data.center.set(this.m_sweep.localCenter);
|
|
};
|
|
/**
|
|
* This resets the mass properties to the sum of the mass properties of the
|
|
* fixtures. This normally does not need to be called unless you called
|
|
* SetMassData to override the mass and you later want to reset the mass.
|
|
*/
|
|
Body.prototype.resetMassData = function () {
|
|
// Compute mass data from shapes. Each shape has its own density.
|
|
this.m_mass = 0.0;
|
|
this.m_invMass = 0.0;
|
|
this.m_I = 0.0;
|
|
this.m_invI = 0.0;
|
|
this.m_sweep.localCenter.setZero();
|
|
// Static and kinematic bodies have zero mass.
|
|
if (this.isStatic() || this.isKinematic()) {
|
|
this.m_sweep.c0.set(this.m_xf.p);
|
|
this.m_sweep.c.set(this.m_xf.p);
|
|
this.m_sweep.a0 = this.m_sweep.a;
|
|
return;
|
|
}
|
|
// Accumulate mass over all fixtures.
|
|
var localCenter = Vec2.zero();
|
|
for (var f = this.m_fixtureList; f; f = f.m_next) {
|
|
if (f.m_density == 0.0) {
|
|
continue;
|
|
}
|
|
var massData = new MassData();
|
|
f.getMassData(massData);
|
|
this.m_mass += massData.mass;
|
|
localCenter.addMul(massData.mass, massData.center);
|
|
this.m_I += massData.I;
|
|
}
|
|
// Compute center of mass.
|
|
if (this.m_mass > 0.0) {
|
|
this.m_invMass = 1.0 / this.m_mass;
|
|
localCenter.mul(this.m_invMass);
|
|
}
|
|
else {
|
|
// Force all dynamic bodies to have a positive mass.
|
|
this.m_mass = 1.0;
|
|
this.m_invMass = 1.0;
|
|
}
|
|
if (this.m_I > 0.0 && this.m_fixedRotationFlag == false) {
|
|
// Center the inertia about the center of mass.
|
|
this.m_I -= this.m_mass * Vec2.dot(localCenter, localCenter);
|
|
this.m_invI = 1.0 / this.m_I;
|
|
}
|
|
else {
|
|
this.m_I = 0.0;
|
|
this.m_invI = 0.0;
|
|
}
|
|
// Move center of mass.
|
|
var oldCenter = Vec2.clone(this.m_sweep.c);
|
|
this.m_sweep.setLocalCenter(localCenter, this.m_xf);
|
|
// Update center of mass velocity.
|
|
this.m_linearVelocity.add(Vec2.cross(this.m_angularVelocity, Vec2.sub(this.m_sweep.c, oldCenter)));
|
|
};
|
|
/**
|
|
* Set the mass properties to override the mass properties of the fixtures. Note
|
|
* that this changes the center of mass position. Note that creating or
|
|
* destroying fixtures can also alter the mass. This function has no effect if
|
|
* the body isn't dynamic.
|
|
*
|
|
* @param massData The mass properties.
|
|
*/
|
|
Body.prototype.setMassData = function (massData) {
|
|
if (this.isWorldLocked() == true) {
|
|
return;
|
|
}
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
this.m_invMass = 0.0;
|
|
this.m_I = 0.0;
|
|
this.m_invI = 0.0;
|
|
this.m_mass = massData.mass;
|
|
if (this.m_mass <= 0.0) {
|
|
this.m_mass = 1.0;
|
|
}
|
|
this.m_invMass = 1.0 / this.m_mass;
|
|
if (massData.I > 0.0 && this.m_fixedRotationFlag == false) {
|
|
this.m_I = massData.I - this.m_mass
|
|
* Vec2.dot(massData.center, massData.center);
|
|
this.m_invI = 1.0 / this.m_I;
|
|
}
|
|
// Move center of mass.
|
|
var oldCenter = Vec2.clone(this.m_sweep.c);
|
|
this.m_sweep.setLocalCenter(massData.center, this.m_xf);
|
|
// Update center of mass velocity.
|
|
this.m_linearVelocity.add(Vec2.cross(this.m_angularVelocity, Vec2.sub(this.m_sweep.c, oldCenter)));
|
|
};
|
|
/**
|
|
* Apply a force at a world point. If the force is not applied at the center of
|
|
* mass, it will generate a torque and affect the angular velocity. This wakes
|
|
* up the body.
|
|
*
|
|
* @param force The world force vector, usually in Newtons (N).
|
|
* @param point The world position of the point of application.
|
|
* @param wake Also wake up the body
|
|
*/
|
|
Body.prototype.applyForce = function (force, point, wake) {
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
if (wake && this.m_awakeFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
// Don't accumulate a force if the body is sleeping.
|
|
if (this.m_awakeFlag) {
|
|
this.m_force.add(force);
|
|
this.m_torque += Vec2.cross(Vec2.sub(point, this.m_sweep.c), force);
|
|
}
|
|
};
|
|
/**
|
|
* Apply a force to the center of mass. This wakes up the body.
|
|
*
|
|
* @param force The world force vector, usually in Newtons (N).
|
|
* @param wake Also wake up the body
|
|
*/
|
|
Body.prototype.applyForceToCenter = function (force, wake) {
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
if (wake && this.m_awakeFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
// Don't accumulate a force if the body is sleeping
|
|
if (this.m_awakeFlag) {
|
|
this.m_force.add(force);
|
|
}
|
|
};
|
|
/**
|
|
* Apply a torque. This affects the angular velocity without affecting the
|
|
* linear velocity of the center of mass. This wakes up the body.
|
|
*
|
|
* @param torque About the z-axis (out of the screen), usually in N-m.
|
|
* @param wake Also wake up the body
|
|
*/
|
|
Body.prototype.applyTorque = function (torque, wake) {
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
if (wake && this.m_awakeFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
// Don't accumulate a force if the body is sleeping
|
|
if (this.m_awakeFlag) {
|
|
this.m_torque += torque;
|
|
}
|
|
};
|
|
/**
|
|
* Apply an impulse at a point. This immediately modifies the velocity. It also
|
|
* modifies the angular velocity if the point of application is not at the
|
|
* center of mass. This wakes up the body.
|
|
*
|
|
* @param impulse The world impulse vector, usually in N-seconds or kg-m/s.
|
|
* @param point The world position of the point of application.
|
|
* @param wake Also wake up the body
|
|
*/
|
|
Body.prototype.applyLinearImpulse = function (impulse, point, wake) {
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
if (wake && this.m_awakeFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
// Don't accumulate velocity if the body is sleeping
|
|
if (this.m_awakeFlag) {
|
|
this.m_linearVelocity.addMul(this.m_invMass, impulse);
|
|
this.m_angularVelocity += this.m_invI * Vec2.cross(Vec2.sub(point, this.m_sweep.c), impulse);
|
|
}
|
|
};
|
|
/**
|
|
* Apply an angular impulse.
|
|
*
|
|
* @param impulse The angular impulse in units of kg*m*m/s
|
|
* @param wake Also wake up the body
|
|
*/
|
|
Body.prototype.applyAngularImpulse = function (impulse, wake) {
|
|
if (this.m_type != DYNAMIC) {
|
|
return;
|
|
}
|
|
if (wake && this.m_awakeFlag == false) {
|
|
this.setAwake(true);
|
|
}
|
|
// Don't accumulate velocity if the body is sleeping
|
|
if (this.m_awakeFlag) {
|
|
this.m_angularVelocity += this.m_invI * impulse;
|
|
}
|
|
};
|
|
/**
|
|
* This is used to prevent connected bodies (by joints) from colliding,
|
|
* depending on the joint's collideConnected flag.
|
|
*/
|
|
Body.prototype.shouldCollide = function (that) {
|
|
// At least one body should be dynamic.
|
|
if (this.m_type != DYNAMIC && that.m_type != DYNAMIC) {
|
|
return false;
|
|
}
|
|
// Does a joint prevent collision?
|
|
for (var jn = this.m_jointList; jn; jn = jn.next) {
|
|
if (jn.other == that) {
|
|
if (jn.joint.m_collideConnected == false) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
};
|
|
/**
|
|
* @internal Used for deserialize.
|
|
*/
|
|
Body.prototype._addFixture = function (fixture) {
|
|
if (this.isWorldLocked() == true) {
|
|
return null;
|
|
}
|
|
if (this.m_activeFlag) {
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
fixture.createProxies(broadPhase, this.m_xf);
|
|
}
|
|
fixture.m_next = this.m_fixtureList;
|
|
this.m_fixtureList = fixture;
|
|
// Adjust mass properties if needed.
|
|
if (fixture.m_density > 0.0) {
|
|
this.resetMassData();
|
|
}
|
|
// Let the world know we have a new fixture. This will cause new contacts
|
|
// to be created at the beginning of the next time step.
|
|
this.m_world.m_newFixture = true;
|
|
return fixture;
|
|
};
|
|
Body.prototype.createFixture = function (shape, fixdef) {
|
|
if (this.isWorldLocked() == true) {
|
|
return null;
|
|
}
|
|
var fixture = new Fixture(this, shape, fixdef);
|
|
this._addFixture(fixture);
|
|
return fixture;
|
|
};
|
|
/**
|
|
* Destroy a fixture. This removes the fixture from the broad-phase and destroys
|
|
* all contacts associated with this fixture. This will automatically adjust the
|
|
* mass of the body if the body is dynamic and the fixture has positive density.
|
|
* All fixtures attached to a body are implicitly destroyed when the body is
|
|
* destroyed.
|
|
*
|
|
* Warning: This function is locked during callbacks.
|
|
*
|
|
* @param fixture The fixture to be removed.
|
|
*/
|
|
Body.prototype.destroyFixture = function (fixture) {
|
|
if (this.isWorldLocked() == true) {
|
|
return;
|
|
}
|
|
if (this.m_fixtureList === fixture) {
|
|
this.m_fixtureList = fixture.m_next;
|
|
}
|
|
else {
|
|
var node = this.m_fixtureList;
|
|
while (node != null) {
|
|
if (node.m_next === fixture) {
|
|
node.m_next = fixture.m_next;
|
|
break;
|
|
}
|
|
node = node.m_next;
|
|
}
|
|
}
|
|
// Destroy any contacts associated with the fixture.
|
|
var edge = this.m_contactList;
|
|
while (edge) {
|
|
var c = edge.contact;
|
|
edge = edge.next;
|
|
var fixtureA = c.getFixtureA();
|
|
var fixtureB = c.getFixtureB();
|
|
if (fixture == fixtureA || fixture == fixtureB) {
|
|
// This destroys the contact and removes it from
|
|
// this body's contact list.
|
|
this.m_world.destroyContact(c);
|
|
}
|
|
}
|
|
if (this.m_activeFlag) {
|
|
var broadPhase = this.m_world.m_broadPhase;
|
|
fixture.destroyProxies(broadPhase);
|
|
}
|
|
fixture.m_body = null;
|
|
fixture.m_next = null;
|
|
this.m_world.publish('remove-fixture', fixture);
|
|
// Reset the mass data.
|
|
this.resetMassData();
|
|
};
|
|
/**
|
|
* Get the corresponding world point of a local point.
|
|
*/
|
|
Body.prototype.getWorldPoint = function (localPoint) {
|
|
return Transform.mulVec2(this.m_xf, localPoint);
|
|
};
|
|
/**
|
|
* Get the corresponding world vector of a local vector.
|
|
*/
|
|
Body.prototype.getWorldVector = function (localVector) {
|
|
return Rot.mulVec2(this.m_xf.q, localVector);
|
|
};
|
|
/**
|
|
* Gets the corresponding local point of a world point.
|
|
*/
|
|
Body.prototype.getLocalPoint = function (worldPoint) {
|
|
return Transform.mulTVec2(this.m_xf, worldPoint);
|
|
};
|
|
/**
|
|
* Gets the corresponding local vector of a world vector.
|
|
*/
|
|
Body.prototype.getLocalVector = function (worldVector) {
|
|
return Rot.mulTVec2(this.m_xf.q, worldVector);
|
|
};
|
|
Body.STATIC = STATIC;
|
|
Body.KINEMATIC = KINEMATIC;
|
|
Body.DYNAMIC = DYNAMIC;
|
|
return Body;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A 2-by-2 matrix. Stored in column-major order.
|
|
*/
|
|
var Mat22 = /** @class */ (function () {
|
|
function Mat22(a, b, c, d) {
|
|
if (typeof a === 'object' && a !== null) {
|
|
this.ex = Vec2.clone(a);
|
|
this.ey = Vec2.clone(b);
|
|
}
|
|
else if (typeof a === 'number') {
|
|
this.ex = Vec2.neo(a, c);
|
|
this.ey = Vec2.neo(b, d);
|
|
}
|
|
else {
|
|
this.ex = Vec2.zero();
|
|
this.ey = Vec2.zero();
|
|
}
|
|
}
|
|
Mat22.prototype.toString = function () {
|
|
return JSON.stringify(this);
|
|
};
|
|
Mat22.isValid = function (o) {
|
|
return o && Vec2.isValid(o.ex) && Vec2.isValid(o.ey);
|
|
};
|
|
Mat22.assert = function (o) {
|
|
return;
|
|
};
|
|
Mat22.prototype.set = function (a, b, c, d) {
|
|
if (typeof a === 'number' && typeof b === 'number' && typeof c === 'number'
|
|
&& typeof d === 'number') {
|
|
this.ex.set(a, c);
|
|
this.ey.set(b, d);
|
|
}
|
|
else if (typeof a === 'object' && typeof b === 'object') {
|
|
this.ex.set(a);
|
|
this.ey.set(b);
|
|
}
|
|
else if (typeof a === 'object') {
|
|
this.ex.set(a.ex);
|
|
this.ey.set(a.ey);
|
|
}
|
|
else ;
|
|
};
|
|
Mat22.prototype.setIdentity = function () {
|
|
this.ex.x = 1.0;
|
|
this.ey.x = 0.0;
|
|
this.ex.y = 0.0;
|
|
this.ey.y = 1.0;
|
|
};
|
|
Mat22.prototype.setZero = function () {
|
|
this.ex.x = 0.0;
|
|
this.ey.x = 0.0;
|
|
this.ex.y = 0.0;
|
|
this.ey.y = 0.0;
|
|
};
|
|
Mat22.prototype.getInverse = function () {
|
|
var a = this.ex.x;
|
|
var b = this.ey.x;
|
|
var c = this.ex.y;
|
|
var d = this.ey.y;
|
|
var det = a * d - b * c;
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
var imx = new Mat22();
|
|
imx.ex.x = det * d;
|
|
imx.ey.x = -det * b;
|
|
imx.ex.y = -det * c;
|
|
imx.ey.y = det * a;
|
|
return imx;
|
|
};
|
|
/**
|
|
* Solve A * x = b, where b is a column vector. This is more efficient than
|
|
* computing the inverse in one-shot cases.
|
|
*/
|
|
Mat22.prototype.solve = function (v) {
|
|
var a = this.ex.x;
|
|
var b = this.ey.x;
|
|
var c = this.ex.y;
|
|
var d = this.ey.y;
|
|
var det = a * d - b * c;
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
var w = Vec2.zero();
|
|
w.x = det * (d * v.x - b * v.y);
|
|
w.y = det * (a * v.y - c * v.x);
|
|
return w;
|
|
};
|
|
Mat22.mul = function (mx, v) {
|
|
if (v && 'x' in v && 'y' in v) {
|
|
var x = mx.ex.x * v.x + mx.ey.x * v.y;
|
|
var y = mx.ex.y * v.x + mx.ey.y * v.y;
|
|
return Vec2.neo(x, y);
|
|
}
|
|
else if (v && 'ex' in v && 'ey' in v) { // Mat22
|
|
// return new Mat22(Vec2.mul(mx, v.ex), Vec2.mul(mx, v.ey));
|
|
var a = mx.ex.x * v.ex.x + mx.ey.x * v.ex.y;
|
|
var b = mx.ex.x * v.ey.x + mx.ey.x * v.ey.y;
|
|
var c = mx.ex.y * v.ex.x + mx.ey.y * v.ex.y;
|
|
var d = mx.ex.y * v.ey.x + mx.ey.y * v.ey.y;
|
|
return new Mat22(a, b, c, d);
|
|
}
|
|
};
|
|
Mat22.mulVec2 = function (mx, v) {
|
|
var x = mx.ex.x * v.x + mx.ey.x * v.y;
|
|
var y = mx.ex.y * v.x + mx.ey.y * v.y;
|
|
return Vec2.neo(x, y);
|
|
};
|
|
Mat22.mulMat22 = function (mx, v) {
|
|
// return new Mat22(Vec2.mul(mx, v.ex), Vec2.mul(mx, v.ey));
|
|
var a = mx.ex.x * v.ex.x + mx.ey.x * v.ex.y;
|
|
var b = mx.ex.x * v.ey.x + mx.ey.x * v.ey.y;
|
|
var c = mx.ex.y * v.ex.x + mx.ey.y * v.ex.y;
|
|
var d = mx.ex.y * v.ey.x + mx.ey.y * v.ey.y;
|
|
return new Mat22(a, b, c, d);
|
|
};
|
|
Mat22.mulT = function (mx, v) {
|
|
if (v && 'x' in v && 'y' in v) { // Vec2
|
|
return Vec2.neo(Vec2.dot(v, mx.ex), Vec2.dot(v, mx.ey));
|
|
}
|
|
else if (v && 'ex' in v && 'ey' in v) { // Mat22
|
|
var c1 = Vec2.neo(Vec2.dot(mx.ex, v.ex), Vec2.dot(mx.ey, v.ex));
|
|
var c2 = Vec2.neo(Vec2.dot(mx.ex, v.ey), Vec2.dot(mx.ey, v.ey));
|
|
return new Mat22(c1, c2);
|
|
}
|
|
};
|
|
Mat22.mulTVec2 = function (mx, v) {
|
|
return Vec2.neo(Vec2.dot(v, mx.ex), Vec2.dot(v, mx.ey));
|
|
};
|
|
Mat22.mulTMat22 = function (mx, v) {
|
|
var c1 = Vec2.neo(Vec2.dot(mx.ex, v.ex), Vec2.dot(mx.ey, v.ex));
|
|
var c2 = Vec2.neo(Vec2.dot(mx.ex, v.ey), Vec2.dot(mx.ey, v.ey));
|
|
return new Mat22(c1, c2);
|
|
};
|
|
Mat22.abs = function (mx) {
|
|
return new Mat22(Vec2.abs(mx.ex), Vec2.abs(mx.ey));
|
|
};
|
|
Mat22.add = function (mx1, mx2) {
|
|
return new Mat22(Vec2.add(mx1.ex, mx2.ex), Vec2.add(mx1.ey, mx2.ey));
|
|
};
|
|
return Mat22;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var ManifoldType;
|
|
(function (ManifoldType) {
|
|
ManifoldType[ManifoldType["e_circles"] = 0] = "e_circles";
|
|
ManifoldType[ManifoldType["e_faceA"] = 1] = "e_faceA";
|
|
ManifoldType[ManifoldType["e_faceB"] = 2] = "e_faceB";
|
|
})(ManifoldType || (ManifoldType = {}));
|
|
var ContactFeatureType;
|
|
(function (ContactFeatureType) {
|
|
ContactFeatureType[ContactFeatureType["e_vertex"] = 0] = "e_vertex";
|
|
ContactFeatureType[ContactFeatureType["e_face"] = 1] = "e_face";
|
|
})(ContactFeatureType || (ContactFeatureType = {}));
|
|
/**
|
|
* A manifold for two touching convex shapes. Manifolds are created in `evaluate`
|
|
* method of Contact subclasses.
|
|
*
|
|
* Supported manifold types are e_faceA or e_faceB for clip point versus plane
|
|
* with radius and e_circles point versus point with radius.
|
|
*
|
|
* We store contacts in this way so that position correction can account for
|
|
* movement, which is critical for continuous physics. All contact scenarios
|
|
* must be expressed in one of these types. This structure is stored across time
|
|
* steps, so we keep it small.
|
|
*
|
|
* @prop type e_circle, e_faceA, e_faceB
|
|
* @prop localPoint Usage depends on manifold type:<br>
|
|
* e_circles: the local center of circleA <br>
|
|
* e_faceA: the center of faceA <br>
|
|
* e_faceB: the center of faceB
|
|
* @prop localNormal Usage depends on manifold type:<br>
|
|
* e_circles: not used <br>
|
|
* e_faceA: the normal on polygonA <br>
|
|
* e_faceB: the normal on polygonB
|
|
* @prop points The points of contact {ManifoldPoint[]}
|
|
* @prop pointCount The number of manifold points
|
|
*/
|
|
var Manifold = /** @class */ (function () {
|
|
function Manifold() {
|
|
this.localNormal = Vec2.zero();
|
|
this.localPoint = Vec2.zero();
|
|
this.points = [new ManifoldPoint(), new ManifoldPoint()];
|
|
this.pointCount = 0;
|
|
}
|
|
/**
|
|
* Evaluate the manifold with supplied transforms. This assumes modest motion
|
|
* from the original state. This does not change the point count, impulses, etc.
|
|
* The radii must come from the shapes that generated the manifold.
|
|
*/
|
|
Manifold.prototype.getWorldManifold = function (wm, xfA, radiusA, xfB, radiusB) {
|
|
if (this.pointCount == 0) {
|
|
return;
|
|
}
|
|
wm = wm || new WorldManifold();
|
|
var normal = wm.normal;
|
|
var points = wm.points;
|
|
var separations = wm.separations;
|
|
// TODO: improve
|
|
switch (this.type) {
|
|
case ManifoldType.e_circles: {
|
|
normal = Vec2.neo(1.0, 0.0);
|
|
var pointA = Transform.mulVec2(xfA, this.localPoint);
|
|
var pointB = Transform.mulVec2(xfB, this.points[0].localPoint);
|
|
var dist = Vec2.sub(pointB, pointA);
|
|
if (Vec2.lengthSquared(dist) > math.EPSILON * math.EPSILON) {
|
|
normal.set(dist);
|
|
normal.normalize();
|
|
}
|
|
var cA = pointA.clone().addMul(radiusA, normal);
|
|
var cB = pointB.clone().addMul(-radiusB, normal);
|
|
points[0] = Vec2.mid(cA, cB);
|
|
separations[0] = Vec2.dot(Vec2.sub(cB, cA), normal);
|
|
points.length = 1;
|
|
separations.length = 1;
|
|
break;
|
|
}
|
|
case ManifoldType.e_faceA: {
|
|
normal = Rot.mulVec2(xfA.q, this.localNormal);
|
|
var planePoint = Transform.mulVec2(xfA, this.localPoint);
|
|
for (var i = 0; i < this.pointCount; ++i) {
|
|
var clipPoint = Transform.mulVec2(xfB, this.points[i].localPoint);
|
|
var cA = Vec2.clone(clipPoint).addMul(radiusA - Vec2.dot(Vec2.sub(clipPoint, planePoint), normal), normal);
|
|
var cB = Vec2.clone(clipPoint).subMul(radiusB, normal);
|
|
points[i] = Vec2.mid(cA, cB);
|
|
separations[i] = Vec2.dot(Vec2.sub(cB, cA), normal);
|
|
}
|
|
points.length = this.pointCount;
|
|
separations.length = this.pointCount;
|
|
break;
|
|
}
|
|
case ManifoldType.e_faceB: {
|
|
normal = Rot.mulVec2(xfB.q, this.localNormal);
|
|
var planePoint = Transform.mulVec2(xfB, this.localPoint);
|
|
for (var i = 0; i < this.pointCount; ++i) {
|
|
var clipPoint = Transform.mulVec2(xfA, this.points[i].localPoint);
|
|
var cB = Vec2.combine(1, clipPoint, radiusB - Vec2.dot(Vec2.sub(clipPoint, planePoint), normal), normal);
|
|
var cA = Vec2.combine(1, clipPoint, -radiusA, normal);
|
|
points[i] = Vec2.mid(cA, cB);
|
|
separations[i] = Vec2.dot(Vec2.sub(cA, cB), normal);
|
|
}
|
|
points.length = this.pointCount;
|
|
separations.length = this.pointCount;
|
|
// Ensure normal points from A to B.
|
|
normal.mul(-1);
|
|
break;
|
|
}
|
|
}
|
|
wm.normal = normal;
|
|
wm.points = points;
|
|
wm.separations = separations;
|
|
return wm;
|
|
};
|
|
return Manifold;
|
|
}());
|
|
/**
|
|
* A manifold point is a contact point belonging to a contact manifold. It holds
|
|
* details related to the geometry and dynamics of the contact points.
|
|
*
|
|
* This structure is stored across time steps, so we keep it small.
|
|
*
|
|
* Note: impulses are used for internal caching and may not provide reliable
|
|
* contact forces, especially for high speed collisions.
|
|
*/
|
|
var ManifoldPoint = /** @class */ (function () {
|
|
function ManifoldPoint() {
|
|
/**
|
|
* Usage depends on manifold type.
|
|
* e_circles: the local center of circleB,
|
|
* e_faceA: the local center of cirlceB or the clip point of polygonB,
|
|
* e_faceB: the clip point of polygonA.
|
|
*/
|
|
this.localPoint = Vec2.zero();
|
|
/**
|
|
* The non-penetration impulse
|
|
*/
|
|
this.normalImpulse = 0;
|
|
/**
|
|
* The friction impulse
|
|
*/
|
|
this.tangentImpulse = 0;
|
|
/**
|
|
* Uniquely identifies a contact point between two shapes to facilatate warm starting
|
|
*/
|
|
this.id = new ContactID();
|
|
}
|
|
return ManifoldPoint;
|
|
}());
|
|
/**
|
|
* Contact ids to facilitate warm starting.
|
|
*/
|
|
var ContactID = /** @class */ (function () {
|
|
function ContactID() {
|
|
this.cf = new ContactFeature();
|
|
}
|
|
Object.defineProperty(ContactID.prototype, "key", {
|
|
/**
|
|
* Used to quickly compare contact ids.
|
|
*/
|
|
get: function () {
|
|
return this.cf.indexA + this.cf.indexB * 4 + this.cf.typeA * 16 + this.cf.typeB * 64;
|
|
},
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
ContactID.prototype.set = function (o) {
|
|
// this.key = o.key;
|
|
this.cf.set(o.cf);
|
|
};
|
|
return ContactID;
|
|
}());
|
|
/**
|
|
* The features that intersect to form the contact point.
|
|
*/
|
|
var ContactFeature = /** @class */ (function () {
|
|
function ContactFeature() {
|
|
}
|
|
ContactFeature.prototype.set = function (o) {
|
|
this.indexA = o.indexA;
|
|
this.indexB = o.indexB;
|
|
this.typeA = o.typeA;
|
|
this.typeB = o.typeB;
|
|
};
|
|
return ContactFeature;
|
|
}());
|
|
/**
|
|
* This is used to compute the current state of a contact manifold.
|
|
*/
|
|
var WorldManifold = /** @class */ (function () {
|
|
function WorldManifold() {
|
|
/**
|
|
* World contact point (point of intersection)
|
|
*/
|
|
this.points = []; // [maxManifoldPoints]
|
|
/**
|
|
* A negative value indicates overlap, in meters
|
|
*/
|
|
this.separations = []; // [maxManifoldPoints]
|
|
}
|
|
return WorldManifold;
|
|
}());
|
|
/**
|
|
* This is used for determining the state of contact points.
|
|
*/
|
|
var PointState;
|
|
(function (PointState) {
|
|
/** Point does not exist */
|
|
PointState[PointState["nullState"] = 0] = "nullState";
|
|
/** Point was added in the update */
|
|
PointState[PointState["addState"] = 1] = "addState";
|
|
/** Point persisted across the update */
|
|
PointState[PointState["persistState"] = 2] = "persistState";
|
|
/** Point was removed in the update */
|
|
PointState[PointState["removeState"] = 3] = "removeState";
|
|
})(PointState || (PointState = {}));
|
|
/**
|
|
* Compute the point states given two manifolds. The states pertain to the
|
|
* transition from manifold1 to manifold2. So state1 is either persist or remove
|
|
* while state2 is either add or persist.
|
|
*/
|
|
function getPointStates(state1, state2, manifold1, manifold2) {
|
|
// state1, state2: PointState[Settings.maxManifoldPoints]
|
|
// for (var i = 0; i < Settings.maxManifoldPoints; ++i) {
|
|
// state1[i] = PointState.nullState;
|
|
// state2[i] = PointState.nullState;
|
|
// }
|
|
// Detect persists and removes.
|
|
for (var i = 0; i < manifold1.pointCount; ++i) {
|
|
var id = manifold1.points[i].id;
|
|
state1[i] = PointState.removeState;
|
|
for (var j = 0; j < manifold2.pointCount; ++j) {
|
|
if (manifold2.points[j].id.key == id.key) {
|
|
state1[i] = PointState.persistState;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
// Detect persists and adds.
|
|
for (var i = 0; i < manifold2.pointCount; ++i) {
|
|
var id = manifold2.points[i].id;
|
|
state2[i] = PointState.addState;
|
|
for (var j = 0; j < manifold1.pointCount; ++j) {
|
|
if (manifold1.points[j].id.key == id.key) {
|
|
state2[i] = PointState.persistState;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
/**
|
|
* Used for computing contact manifolds.
|
|
*/
|
|
var ClipVertex = /** @class */ (function () {
|
|
function ClipVertex() {
|
|
this.v = Vec2.zero();
|
|
this.id = new ContactID();
|
|
}
|
|
ClipVertex.prototype.set = function (o) {
|
|
this.v.set(o.v);
|
|
this.id.set(o.id);
|
|
};
|
|
return ClipVertex;
|
|
}());
|
|
/**
|
|
* Clipping for contact manifolds. Sutherland-Hodgman clipping.
|
|
*/
|
|
function clipSegmentToLine(vOut, vIn, normal, offset, vertexIndexA) {
|
|
// Start with no output points
|
|
var numOut = 0;
|
|
// Calculate the distance of end points to the line
|
|
var distance0 = Vec2.dot(normal, vIn[0].v) - offset;
|
|
var distance1 = Vec2.dot(normal, vIn[1].v) - offset;
|
|
// If the points are behind the plane
|
|
if (distance0 <= 0.0)
|
|
vOut[numOut++].set(vIn[0]);
|
|
if (distance1 <= 0.0)
|
|
vOut[numOut++].set(vIn[1]);
|
|
// If the points are on different sides of the plane
|
|
if (distance0 * distance1 < 0.0) {
|
|
// Find intersection point of edge and plane
|
|
var interp = distance0 / (distance0 - distance1);
|
|
vOut[numOut].v.setCombine(1 - interp, vIn[0].v, interp, vIn[1].v);
|
|
// VertexA is hitting edgeB.
|
|
vOut[numOut].id.cf.indexA = vertexIndexA;
|
|
vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
|
|
vOut[numOut].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
vOut[numOut].id.cf.typeB = ContactFeatureType.e_face;
|
|
++numOut;
|
|
}
|
|
return numOut;
|
|
}
|
|
|
|
var stats = {
|
|
gjkCalls: 0,
|
|
gjkIters: 0,
|
|
gjkMaxIters: 0,
|
|
toiTime: 0,
|
|
toiMaxTime: 0,
|
|
toiCalls: 0,
|
|
toiIters: 0,
|
|
toiMaxIters: 0,
|
|
toiRootIters: 0,
|
|
toiMaxRootIters: 0,
|
|
toString: function (newline) {
|
|
newline = typeof newline === 'string' ? newline : '\n';
|
|
var string = "";
|
|
for (var name_1 in this) {
|
|
if (typeof this[name_1] !== 'function' && typeof this[name_1] !== 'object') {
|
|
string += name_1 + ': ' + this[name_1] + newline;
|
|
}
|
|
}
|
|
return string;
|
|
}
|
|
};
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
|
|
*/
|
|
stats.gjkCalls = 0;
|
|
stats.gjkIters = 0;
|
|
stats.gjkMaxIters = 0;
|
|
/**
|
|
* Input for Distance. You have to option to use the shape radii in the
|
|
* computation. Even
|
|
*/
|
|
var DistanceInput = /** @class */ (function () {
|
|
function DistanceInput() {
|
|
this.proxyA = new DistanceProxy();
|
|
this.proxyB = new DistanceProxy();
|
|
this.transformA = null;
|
|
this.transformB = null;
|
|
this.useRadii = false;
|
|
}
|
|
return DistanceInput;
|
|
}());
|
|
/**
|
|
* Output for Distance.
|
|
*
|
|
* @prop {Vec2} pointA closest point on shapeA
|
|
* @prop {Vec2} pointB closest point on shapeB
|
|
* @prop distance
|
|
* @prop iterations number of GJK iterations used
|
|
*/
|
|
var DistanceOutput = /** @class */ (function () {
|
|
function DistanceOutput() {
|
|
this.pointA = Vec2.zero();
|
|
this.pointB = Vec2.zero();
|
|
}
|
|
return DistanceOutput;
|
|
}());
|
|
/**
|
|
* Used to warm start Distance. Set count to zero on first call.
|
|
*
|
|
* @prop {number} metric length or area
|
|
* @prop {array} indexA vertices on shape A
|
|
* @prop {array} indexB vertices on shape B
|
|
* @prop {number} count
|
|
*/
|
|
var SimplexCache = /** @class */ (function () {
|
|
function SimplexCache() {
|
|
this.metric = 0;
|
|
this.indexA = [];
|
|
this.indexB = [];
|
|
this.count = 0;
|
|
}
|
|
return SimplexCache;
|
|
}());
|
|
/**
|
|
* Compute the closest points between two shapes. Supports any combination of:
|
|
* CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. On
|
|
* the first call set SimplexCache.count to zero.
|
|
*/
|
|
function Distance(output, cache, input) {
|
|
++stats.gjkCalls;
|
|
var proxyA = input.proxyA;
|
|
var proxyB = input.proxyB;
|
|
var xfA = input.transformA;
|
|
var xfB = input.transformB;
|
|
// Initialize the simplex.
|
|
var simplex = new Simplex();
|
|
simplex.readCache(cache, proxyA, xfA, proxyB, xfB);
|
|
// Get simplex vertices as an array.
|
|
var vertices = simplex.m_v;
|
|
var k_maxIters = Settings.maxDistnceIterations;
|
|
// These store the vertices of the last simplex so that we
|
|
// can check for duplicates and prevent cycling.
|
|
var saveA = [];
|
|
var saveB = []; // int[3]
|
|
var saveCount = 0;
|
|
// Main iteration loop.
|
|
var iter = 0;
|
|
while (iter < k_maxIters) {
|
|
// Copy simplex so we can identify duplicates.
|
|
saveCount = simplex.m_count;
|
|
for (var i = 0; i < saveCount; ++i) {
|
|
saveA[i] = vertices[i].indexA;
|
|
saveB[i] = vertices[i].indexB;
|
|
}
|
|
simplex.solve();
|
|
// If we have 3 points, then the origin is in the corresponding triangle.
|
|
if (simplex.m_count === 3) {
|
|
break;
|
|
}
|
|
// Compute closest point.
|
|
var p = simplex.getClosestPoint();
|
|
p.lengthSquared();
|
|
// Get search direction.
|
|
var d = simplex.getSearchDirection();
|
|
// Ensure the search direction is numerically fit.
|
|
if (d.lengthSquared() < math.EPSILON * math.EPSILON) {
|
|
// The origin is probably contained by a line segment
|
|
// or triangle. Thus the shapes are overlapped.
|
|
// We can't return zero here even though there may be overlap.
|
|
// In case the simplex is a point, segment, or triangle it is difficult
|
|
// to determine if the origin is contained in the CSO or very close to it.
|
|
break;
|
|
}
|
|
// Compute a tentative new simplex vertex using support points.
|
|
var vertex = vertices[simplex.m_count]; // SimplexVertex
|
|
vertex.indexA = proxyA.getSupport(Rot.mulTVec2(xfA.q, Vec2.neg(d)));
|
|
vertex.wA = Transform.mulVec2(xfA, proxyA.getVertex(vertex.indexA));
|
|
vertex.indexB = proxyB.getSupport(Rot.mulTVec2(xfB.q, d));
|
|
vertex.wB = Transform.mulVec2(xfB, proxyB.getVertex(vertex.indexB));
|
|
vertex.w = Vec2.sub(vertex.wB, vertex.wA);
|
|
// Iteration count is equated to the number of support point calls.
|
|
++iter;
|
|
++stats.gjkIters;
|
|
// Check for duplicate support points. This is the main termination
|
|
// criteria.
|
|
var duplicate = false;
|
|
for (var i = 0; i < saveCount; ++i) {
|
|
if (vertex.indexA === saveA[i] && vertex.indexB === saveB[i]) {
|
|
duplicate = true;
|
|
break;
|
|
}
|
|
}
|
|
// If we found a duplicate support point we must exit to avoid cycling.
|
|
if (duplicate) {
|
|
break;
|
|
}
|
|
// New vertex is ok and needed.
|
|
++simplex.m_count;
|
|
}
|
|
stats.gjkMaxIters = math.max(stats.gjkMaxIters, iter);
|
|
// Prepare output.
|
|
simplex.getWitnessPoints(output.pointA, output.pointB);
|
|
output.distance = Vec2.distance(output.pointA, output.pointB);
|
|
output.iterations = iter;
|
|
// Cache the simplex.
|
|
simplex.writeCache(cache);
|
|
// Apply radii if requested.
|
|
if (input.useRadii) {
|
|
var rA = proxyA.m_radius;
|
|
var rB = proxyB.m_radius;
|
|
if (output.distance > rA + rB && output.distance > math.EPSILON) {
|
|
// Shapes are still no overlapped.
|
|
// Move the witness points to the outer surface.
|
|
output.distance -= rA + rB;
|
|
var normal = Vec2.sub(output.pointB, output.pointA);
|
|
normal.normalize();
|
|
output.pointA.addMul(rA, normal);
|
|
output.pointB.subMul(rB, normal);
|
|
}
|
|
else {
|
|
// Shapes are overlapped when radii are considered.
|
|
// Move the witness points to the middle.
|
|
var p = Vec2.mid(output.pointA, output.pointB);
|
|
output.pointA.set(p);
|
|
output.pointB.set(p);
|
|
output.distance = 0.0;
|
|
}
|
|
}
|
|
}
|
|
/**
|
|
* A distance proxy is used by the GJK algorithm. It encapsulates any shape.
|
|
*/
|
|
var DistanceProxy = /** @class */ (function () {
|
|
function DistanceProxy() {
|
|
this.m_buffer = []; // Vec2[2]
|
|
this.m_vertices = []; // Vec2[]
|
|
this.m_count = 0;
|
|
this.m_radius = 0;
|
|
}
|
|
DistanceProxy.prototype.getVertexCount = function () {
|
|
return this.m_count;
|
|
};
|
|
DistanceProxy.prototype.getVertex = function (index) {
|
|
return this.m_vertices[index];
|
|
};
|
|
DistanceProxy.prototype.getSupport = function (d) {
|
|
var bestIndex = 0;
|
|
var bestValue = Vec2.dot(this.m_vertices[0], d);
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
var value = Vec2.dot(this.m_vertices[i], d);
|
|
if (value > bestValue) {
|
|
bestIndex = i;
|
|
bestValue = value;
|
|
}
|
|
}
|
|
return bestIndex;
|
|
};
|
|
DistanceProxy.prototype.getSupportVertex = function (d) {
|
|
return this.m_vertices[this.getSupport(d)];
|
|
};
|
|
DistanceProxy.prototype.set = function (shape, index) {
|
|
shape.computeDistanceProxy(this, index);
|
|
};
|
|
return DistanceProxy;
|
|
}());
|
|
var SimplexVertex = /** @class */ (function () {
|
|
function SimplexVertex() {
|
|
this.wA = Vec2.zero(); // support point in proxyA
|
|
this.wB = Vec2.zero(); // support point in proxyB
|
|
this.w = Vec2.zero(); // wB - wA;
|
|
}
|
|
SimplexVertex.prototype.set = function (v) {
|
|
this.indexA = v.indexA;
|
|
this.indexB = v.indexB;
|
|
this.wA = Vec2.clone(v.wA);
|
|
this.wB = Vec2.clone(v.wB);
|
|
this.w = Vec2.clone(v.w);
|
|
this.a = v.a;
|
|
};
|
|
return SimplexVertex;
|
|
}());
|
|
var Simplex = /** @class */ (function () {
|
|
function Simplex() {
|
|
this.m_v1 = new SimplexVertex();
|
|
this.m_v2 = new SimplexVertex();
|
|
this.m_v3 = new SimplexVertex();
|
|
this.m_v = [this.m_v1, this.m_v2, this.m_v3];
|
|
this.m_count;
|
|
}
|
|
Simplex.prototype.print = function () {
|
|
if (this.m_count === 3) {
|
|
return ["+" + this.m_count,
|
|
this.m_v1.a, this.m_v1.wA.x, this.m_v1.wA.y, this.m_v1.wB.x, this.m_v1.wB.y,
|
|
this.m_v2.a, this.m_v2.wA.x, this.m_v2.wA.y, this.m_v2.wB.x, this.m_v2.wB.y,
|
|
this.m_v3.a, this.m_v3.wA.x, this.m_v3.wA.y, this.m_v3.wB.x, this.m_v3.wB.y
|
|
].toString();
|
|
}
|
|
else if (this.m_count === 2) {
|
|
return ["+" + this.m_count,
|
|
this.m_v1.a, this.m_v1.wA.x, this.m_v1.wA.y, this.m_v1.wB.x, this.m_v1.wB.y,
|
|
this.m_v2.a, this.m_v2.wA.x, this.m_v2.wA.y, this.m_v2.wB.x, this.m_v2.wB.y
|
|
].toString();
|
|
}
|
|
else if (this.m_count === 1) {
|
|
return ["+" + this.m_count,
|
|
this.m_v1.a, this.m_v1.wA.x, this.m_v1.wA.y, this.m_v1.wB.x, this.m_v1.wB.y
|
|
].toString();
|
|
}
|
|
else {
|
|
return "+" + this.m_count;
|
|
}
|
|
};
|
|
// (SimplexCache, DistanceProxy, ...)
|
|
Simplex.prototype.readCache = function (cache, proxyA, transformA, proxyB, transformB) {
|
|
// Copy data from cache.
|
|
this.m_count = cache.count;
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
var v = this.m_v[i];
|
|
v.indexA = cache.indexA[i];
|
|
v.indexB = cache.indexB[i];
|
|
var wALocal = proxyA.getVertex(v.indexA);
|
|
var wBLocal = proxyB.getVertex(v.indexB);
|
|
v.wA = Transform.mulVec2(transformA, wALocal);
|
|
v.wB = Transform.mulVec2(transformB, wBLocal);
|
|
v.w = Vec2.sub(v.wB, v.wA);
|
|
v.a = 0.0;
|
|
}
|
|
// Compute the new simplex metric, if it is substantially different than
|
|
// old metric then flush the simplex.
|
|
if (this.m_count > 1) {
|
|
var metric1 = cache.metric;
|
|
var metric2 = this.getMetric();
|
|
if (metric2 < 0.5 * metric1 || 2.0 * metric1 < metric2
|
|
|| metric2 < math.EPSILON) {
|
|
// Reset the simplex.
|
|
this.m_count = 0;
|
|
}
|
|
}
|
|
// If the cache is empty or invalid...
|
|
if (this.m_count === 0) {
|
|
var v = this.m_v[0];
|
|
v.indexA = 0;
|
|
v.indexB = 0;
|
|
var wALocal = proxyA.getVertex(0);
|
|
var wBLocal = proxyB.getVertex(0);
|
|
v.wA = Transform.mulVec2(transformA, wALocal);
|
|
v.wB = Transform.mulVec2(transformB, wBLocal);
|
|
v.w = Vec2.sub(v.wB, v.wA);
|
|
v.a = 1.0;
|
|
this.m_count = 1;
|
|
}
|
|
};
|
|
Simplex.prototype.writeCache = function (cache) {
|
|
cache.metric = this.getMetric();
|
|
cache.count = this.m_count;
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
cache.indexA[i] = this.m_v[i].indexA;
|
|
cache.indexB[i] = this.m_v[i].indexB;
|
|
}
|
|
};
|
|
Simplex.prototype.getSearchDirection = function () {
|
|
switch (this.m_count) {
|
|
case 1:
|
|
return Vec2.neg(this.m_v1.w);
|
|
case 2: {
|
|
var e12 = Vec2.sub(this.m_v2.w, this.m_v1.w);
|
|
var sgn = Vec2.cross(e12, Vec2.neg(this.m_v1.w));
|
|
if (sgn > 0.0) {
|
|
// Origin is left of e12.
|
|
return Vec2.cross(1.0, e12);
|
|
}
|
|
else {
|
|
// Origin is right of e12.
|
|
return Vec2.cross(e12, 1.0);
|
|
}
|
|
}
|
|
default:
|
|
return Vec2.zero();
|
|
}
|
|
};
|
|
Simplex.prototype.getClosestPoint = function () {
|
|
switch (this.m_count) {
|
|
case 0:
|
|
return Vec2.zero();
|
|
case 1:
|
|
return Vec2.clone(this.m_v1.w);
|
|
case 2:
|
|
return Vec2.combine(this.m_v1.a, this.m_v1.w, this.m_v2.a, this.m_v2.w);
|
|
case 3:
|
|
return Vec2.zero();
|
|
default:
|
|
return Vec2.zero();
|
|
}
|
|
};
|
|
Simplex.prototype.getWitnessPoints = function (pA, pB) {
|
|
switch (this.m_count) {
|
|
case 0:
|
|
break;
|
|
case 1:
|
|
pA.set(this.m_v1.wA);
|
|
pB.set(this.m_v1.wB);
|
|
break;
|
|
case 2:
|
|
pA.setCombine(this.m_v1.a, this.m_v1.wA, this.m_v2.a, this.m_v2.wA);
|
|
pB.setCombine(this.m_v1.a, this.m_v1.wB, this.m_v2.a, this.m_v2.wB);
|
|
break;
|
|
case 3:
|
|
pA.setCombine(this.m_v1.a, this.m_v1.wA, this.m_v2.a, this.m_v2.wA);
|
|
pA.addMul(this.m_v3.a, this.m_v3.wA);
|
|
pB.set(pA);
|
|
break;
|
|
}
|
|
};
|
|
Simplex.prototype.getMetric = function () {
|
|
switch (this.m_count) {
|
|
case 0:
|
|
return 0.0;
|
|
case 1:
|
|
return 0.0;
|
|
case 2:
|
|
return Vec2.distance(this.m_v1.w, this.m_v2.w);
|
|
case 3:
|
|
return Vec2.cross(Vec2.sub(this.m_v2.w, this.m_v1.w), Vec2.sub(this.m_v3.w, this.m_v1.w));
|
|
default:
|
|
return 0.0;
|
|
}
|
|
};
|
|
Simplex.prototype.solve = function () {
|
|
switch (this.m_count) {
|
|
case 1:
|
|
break;
|
|
case 2:
|
|
this.solve2();
|
|
break;
|
|
case 3:
|
|
this.solve3();
|
|
break;
|
|
}
|
|
};
|
|
// Solve a line segment using barycentric coordinates.
|
|
//
|
|
// p = a1 * w1 + a2 * w2
|
|
// a1 + a2 = 1
|
|
//
|
|
// The vector from the origin to the closest point on the line is
|
|
// perpendicular to the line.
|
|
// e12 = w2 - w1
|
|
// dot(p, e) = 0
|
|
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
|
|
//
|
|
// 2-by-2 linear system
|
|
// [1 1 ][a1] = [1]
|
|
// [w1.e12 w2.e12][a2] = [0]
|
|
//
|
|
// Define
|
|
// d12_1 = dot(w2, e12)
|
|
// d12_2 = -dot(w1, e12)
|
|
// d12 = d12_1 + d12_2
|
|
//
|
|
// Solution
|
|
// a1 = d12_1 / d12
|
|
// a2 = d12_2 / d12
|
|
Simplex.prototype.solve2 = function () {
|
|
var w1 = this.m_v1.w;
|
|
var w2 = this.m_v2.w;
|
|
var e12 = Vec2.sub(w2, w1);
|
|
// w1 region
|
|
var d12_2 = -Vec2.dot(w1, e12);
|
|
if (d12_2 <= 0.0) {
|
|
// a2 <= 0, so we clamp it to 0
|
|
this.m_v1.a = 1.0;
|
|
this.m_count = 1;
|
|
return;
|
|
}
|
|
// w2 region
|
|
var d12_1 = Vec2.dot(w2, e12);
|
|
if (d12_1 <= 0.0) {
|
|
// a1 <= 0, so we clamp it to 0
|
|
this.m_v2.a = 1.0;
|
|
this.m_count = 1;
|
|
this.m_v1.set(this.m_v2);
|
|
return;
|
|
}
|
|
// Must be in e12 region.
|
|
var inv_d12 = 1.0 / (d12_1 + d12_2);
|
|
this.m_v1.a = d12_1 * inv_d12;
|
|
this.m_v2.a = d12_2 * inv_d12;
|
|
this.m_count = 2;
|
|
};
|
|
// Possible regions:
|
|
// - points[2]
|
|
// - edge points[0]-points[2]
|
|
// - edge points[1]-points[2]
|
|
// - inside the triangle
|
|
Simplex.prototype.solve3 = function () {
|
|
var w1 = this.m_v1.w;
|
|
var w2 = this.m_v2.w;
|
|
var w3 = this.m_v3.w;
|
|
// Edge12
|
|
// [1 1 ][a1] = [1]
|
|
// [w1.e12 w2.e12][a2] = [0]
|
|
// a3 = 0
|
|
var e12 = Vec2.sub(w2, w1);
|
|
var w1e12 = Vec2.dot(w1, e12);
|
|
var w2e12 = Vec2.dot(w2, e12);
|
|
var d12_1 = w2e12;
|
|
var d12_2 = -w1e12;
|
|
// Edge13
|
|
// [1 1 ][a1] = [1]
|
|
// [w1.e13 w3.e13][a3] = [0]
|
|
// a2 = 0
|
|
var e13 = Vec2.sub(w3, w1);
|
|
var w1e13 = Vec2.dot(w1, e13);
|
|
var w3e13 = Vec2.dot(w3, e13);
|
|
var d13_1 = w3e13;
|
|
var d13_2 = -w1e13;
|
|
// Edge23
|
|
// [1 1 ][a2] = [1]
|
|
// [w2.e23 w3.e23][a3] = [0]
|
|
// a1 = 0
|
|
var e23 = Vec2.sub(w3, w2);
|
|
var w2e23 = Vec2.dot(w2, e23);
|
|
var w3e23 = Vec2.dot(w3, e23);
|
|
var d23_1 = w3e23;
|
|
var d23_2 = -w2e23;
|
|
// Triangle123
|
|
var n123 = Vec2.cross(e12, e13);
|
|
var d123_1 = n123 * Vec2.cross(w2, w3);
|
|
var d123_2 = n123 * Vec2.cross(w3, w1);
|
|
var d123_3 = n123 * Vec2.cross(w1, w2);
|
|
// w1 region
|
|
if (d12_2 <= 0.0 && d13_2 <= 0.0) {
|
|
this.m_v1.a = 1.0;
|
|
this.m_count = 1;
|
|
return;
|
|
}
|
|
// e12
|
|
if (d12_1 > 0.0 && d12_2 > 0.0 && d123_3 <= 0.0) {
|
|
var inv_d12 = 1.0 / (d12_1 + d12_2);
|
|
this.m_v1.a = d12_1 * inv_d12;
|
|
this.m_v2.a = d12_2 * inv_d12;
|
|
this.m_count = 2;
|
|
return;
|
|
}
|
|
// e13
|
|
if (d13_1 > 0.0 && d13_2 > 0.0 && d123_2 <= 0.0) {
|
|
var inv_d13 = 1.0 / (d13_1 + d13_2);
|
|
this.m_v1.a = d13_1 * inv_d13;
|
|
this.m_v3.a = d13_2 * inv_d13;
|
|
this.m_count = 2;
|
|
this.m_v2.set(this.m_v3);
|
|
return;
|
|
}
|
|
// w2 region
|
|
if (d12_1 <= 0.0 && d23_2 <= 0.0) {
|
|
this.m_v2.a = 1.0;
|
|
this.m_count = 1;
|
|
this.m_v1.set(this.m_v2);
|
|
return;
|
|
}
|
|
// w3 region
|
|
if (d13_1 <= 0.0 && d23_1 <= 0.0) {
|
|
this.m_v3.a = 1.0;
|
|
this.m_count = 1;
|
|
this.m_v1.set(this.m_v3);
|
|
return;
|
|
}
|
|
// e23
|
|
if (d23_1 > 0.0 && d23_2 > 0.0 && d123_1 <= 0.0) {
|
|
var inv_d23 = 1.0 / (d23_1 + d23_2);
|
|
this.m_v2.a = d23_1 * inv_d23;
|
|
this.m_v3.a = d23_2 * inv_d23;
|
|
this.m_count = 2;
|
|
this.m_v1.set(this.m_v3);
|
|
return;
|
|
}
|
|
// Must be in triangle123
|
|
var inv_d123 = 1.0 / (d123_1 + d123_2 + d123_3);
|
|
this.m_v1.a = d123_1 * inv_d123;
|
|
this.m_v2.a = d123_2 * inv_d123;
|
|
this.m_v3.a = d123_3 * inv_d123;
|
|
this.m_count = 3;
|
|
};
|
|
return Simplex;
|
|
}());
|
|
/**
|
|
* Determine if two generic shapes overlap.
|
|
*/
|
|
function testOverlap(shapeA, indexA, shapeB, indexB, xfA, xfB) {
|
|
var input = new DistanceInput();
|
|
input.proxyA.set(shapeA, indexA);
|
|
input.proxyB.set(shapeB, indexB);
|
|
input.transformA = xfA;
|
|
input.transformB = xfB;
|
|
input.useRadii = true;
|
|
var cache = new SimplexCache();
|
|
var output = new DistanceOutput();
|
|
Distance(output, cache, input);
|
|
return output.distance < 10.0 * math.EPSILON;
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* 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.
|
|
*
|
|
* @prop {Contact} contact The contact
|
|
* @prop {ContactEdge} prev The previous contact edge in the body's contact list
|
|
* @prop {ContactEdge} next The next contact edge in the body's contact list
|
|
* @prop {Body} other Provides quick access to the other body attached.
|
|
*/
|
|
var ContactEdge = /** @class */ (function () {
|
|
function ContactEdge(contact) {
|
|
this.contact = contact;
|
|
}
|
|
return ContactEdge;
|
|
}());
|
|
/**
|
|
* Friction mixing law. The idea is to allow either fixture to drive the
|
|
* restitution to zero. For example, anything slides on ice.
|
|
*/
|
|
function mixFriction(friction1, friction2) {
|
|
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.
|
|
*/
|
|
function mixRestitution(restitution1, restitution2) {
|
|
return restitution1 > restitution2 ? restitution1 : restitution2;
|
|
}
|
|
// TODO: move this to Settings?
|
|
var s_registers = [];
|
|
// TODO: merge with ManifoldPoint?
|
|
var VelocityConstraintPoint = /** @class */ (function () {
|
|
function VelocityConstraintPoint() {
|
|
this.rA = Vec2.zero();
|
|
this.rB = Vec2.zero();
|
|
this.normalImpulse = 0;
|
|
this.tangentImpulse = 0;
|
|
this.normalMass = 0;
|
|
this.tangentMass = 0;
|
|
this.velocityBias = 0;
|
|
}
|
|
return VelocityConstraintPoint;
|
|
}());
|
|
/**
|
|
* 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.
|
|
*/
|
|
var Contact = /** @class */ (function () {
|
|
function Contact(fA, indexA, fB, indexB, evaluateFcn) {
|
|
/** @internal */
|
|
this.m_manifold = new Manifold();
|
|
/** @internal */
|
|
this.m_prev = null;
|
|
/** @internal */
|
|
this.m_next = null;
|
|
/** @internal */
|
|
this.m_toi = 1.0;
|
|
/** @internal */
|
|
this.m_toiCount = 0;
|
|
/** @internal This contact has a valid TOI in m_toi */
|
|
this.m_toiFlag = false;
|
|
/** @internal */
|
|
this.m_tangentSpeed = 0.0;
|
|
/** @internal This contact can be disabled (by user) */
|
|
this.m_enabledFlag = true;
|
|
/** @internal Used when crawling contact graph when forming islands. */
|
|
this.m_islandFlag = false;
|
|
/** @internal Set when the shapes are touching. */
|
|
this.m_touchingFlag = false;
|
|
/** @internal This contact needs filtering because a fixture filter was changed. */
|
|
this.m_filterFlag = false;
|
|
/** @internal This bullet contact had a TOI event */
|
|
this.m_bulletHitFlag = false;
|
|
/** @internal Contact reporting impulse object cache */
|
|
this.m_impulse = new ContactImpulse(this);
|
|
// VelocityConstraint
|
|
/** @internal */ this.v_points = []; // [maxManifoldPoints];
|
|
/** @internal */ this.v_normal = Vec2.zero();
|
|
/** @internal */ this.v_normalMass = new Mat22();
|
|
/** @internal */ this.v_K = new Mat22();
|
|
// PositionConstraint
|
|
/** @internal */ this.p_localPoints = []; // [maxManifoldPoints];
|
|
/** @internal */ this.p_localNormal = Vec2.zero();
|
|
/** @internal */ this.p_localPoint = Vec2.zero();
|
|
/** @internal */ this.p_localCenterA = Vec2.zero();
|
|
/** @internal */ this.p_localCenterB = Vec2.zero();
|
|
// Nodes for connecting bodies.
|
|
this.m_nodeA = new ContactEdge(this);
|
|
this.m_nodeB = new ContactEdge(this);
|
|
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);
|
|
}
|
|
Contact.prototype.initConstraint = function (step) {
|
|
var fixtureA = this.m_fixtureA;
|
|
var fixtureB = this.m_fixtureB;
|
|
var shapeA = fixtureA.getShape();
|
|
var shapeB = fixtureB.getShape();
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
var manifold = this.getManifold();
|
|
var pointCount = manifold.pointCount;
|
|
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;
|
|
this.p_localCenterA = Vec2.clone(bodyA.m_sweep.localCenter);
|
|
this.p_localCenterB = Vec2.clone(bodyB.m_sweep.localCenter);
|
|
this.p_radiusA = shapeA.m_radius;
|
|
this.p_radiusB = shapeB.m_radius;
|
|
this.p_type = manifold.type;
|
|
this.p_localNormal = Vec2.clone(manifold.localNormal);
|
|
this.p_localPoint = Vec2.clone(manifold.localPoint);
|
|
this.p_pointCount = pointCount;
|
|
for (var j = 0; j < pointCount; ++j) {
|
|
var cp = manifold.points[j];
|
|
var vcp = this.v_points[j] = new VelocityConstraintPoint();
|
|
if (step.warmStarting) {
|
|
vcp.normalImpulse = step.dtRatio * cp.normalImpulse;
|
|
vcp.tangentImpulse = step.dtRatio * cp.tangentImpulse;
|
|
}
|
|
else {
|
|
vcp.normalImpulse = 0.0;
|
|
vcp.tangentImpulse = 0.0;
|
|
}
|
|
vcp.rA.setZero();
|
|
vcp.rB.setZero();
|
|
vcp.normalMass = 0.0;
|
|
vcp.tangentMass = 0.0;
|
|
vcp.velocityBias = 0.0;
|
|
this.p_localPoints[j] = Vec2.clone(cp.localPoint);
|
|
}
|
|
};
|
|
/**
|
|
* Get the contact manifold. Do not modify the manifold unless you understand
|
|
* the internals of the library.
|
|
*/
|
|
Contact.prototype.getManifold = function () {
|
|
return this.m_manifold;
|
|
};
|
|
/**
|
|
* Get the world manifold.
|
|
*/
|
|
Contact.prototype.getWorldManifold = function (worldManifold) {
|
|
var bodyA = this.m_fixtureA.getBody();
|
|
var bodyB = this.m_fixtureB.getBody();
|
|
var shapeA = this.m_fixtureA.getShape();
|
|
var shapeB = this.m_fixtureB.getShape();
|
|
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).
|
|
*/
|
|
Contact.prototype.setEnabled = function (flag) {
|
|
this.m_enabledFlag = !!flag;
|
|
};
|
|
/**
|
|
* Has this contact been disabled?
|
|
*/
|
|
Contact.prototype.isEnabled = function () {
|
|
return this.m_enabledFlag;
|
|
};
|
|
/**
|
|
* Is this contact touching?
|
|
*/
|
|
Contact.prototype.isTouching = function () {
|
|
return this.m_touchingFlag;
|
|
};
|
|
/**
|
|
* Get the next contact in the world's contact list.
|
|
*/
|
|
Contact.prototype.getNext = function () {
|
|
return this.m_next;
|
|
};
|
|
/**
|
|
* Get fixture A in this contact.
|
|
*/
|
|
Contact.prototype.getFixtureA = function () {
|
|
return this.m_fixtureA;
|
|
};
|
|
/**
|
|
* Get fixture B in this contact.
|
|
*/
|
|
Contact.prototype.getFixtureB = function () {
|
|
return this.m_fixtureB;
|
|
};
|
|
/**
|
|
* Get the child primitive index for fixture A.
|
|
*/
|
|
Contact.prototype.getChildIndexA = function () {
|
|
return this.m_indexA;
|
|
};
|
|
/**
|
|
* Get the child primitive index for fixture B.
|
|
*/
|
|
Contact.prototype.getChildIndexB = function () {
|
|
return this.m_indexB;
|
|
};
|
|
/**
|
|
* Flag this contact for filtering. Filtering will occur the next time step.
|
|
*/
|
|
Contact.prototype.flagForFiltering = function () {
|
|
this.m_filterFlag = true;
|
|
};
|
|
/**
|
|
* Override the default friction mixture. You can call this in
|
|
* ContactListener.preSolve. This value persists until set or reset.
|
|
*/
|
|
Contact.prototype.setFriction = function (friction) {
|
|
this.m_friction = friction;
|
|
};
|
|
/**
|
|
* Get the friction.
|
|
*/
|
|
Contact.prototype.getFriction = function () {
|
|
return this.m_friction;
|
|
};
|
|
/**
|
|
* Reset the friction mixture to the default value.
|
|
*/
|
|
Contact.prototype.resetFriction = function () {
|
|
this.m_friction = mixFriction(this.m_fixtureA.m_friction, this.m_fixtureB.m_friction);
|
|
};
|
|
/**
|
|
* Override the default restitution mixture. You can call this in
|
|
* ContactListener.preSolve. The value persists until you set or reset.
|
|
*/
|
|
Contact.prototype.setRestitution = function (restitution) {
|
|
this.m_restitution = restitution;
|
|
};
|
|
/**
|
|
* Get the restitution.
|
|
*/
|
|
Contact.prototype.getRestitution = function () {
|
|
return this.m_restitution;
|
|
};
|
|
/**
|
|
* Reset the restitution to the default value.
|
|
*/
|
|
Contact.prototype.resetRestitution = function () {
|
|
this.m_restitution = mixRestitution(this.m_fixtureA.m_restitution, this.m_fixtureB.m_restitution);
|
|
};
|
|
/**
|
|
* Set the desired tangent speed for a conveyor belt behavior. In meters per
|
|
* second.
|
|
*/
|
|
Contact.prototype.setTangentSpeed = function (speed) {
|
|
this.m_tangentSpeed = speed;
|
|
};
|
|
/**
|
|
* Get the desired tangent speed. In meters per second.
|
|
*/
|
|
Contact.prototype.getTangentSpeed = function () {
|
|
return this.m_tangentSpeed;
|
|
};
|
|
/**
|
|
* Called by Update method, and implemented by subclasses.
|
|
*/
|
|
Contact.prototype.evaluate = function (manifold, xfA, xfB) {
|
|
this.m_evaluateFcn(manifold, xfA, this.m_fixtureA, this.m_indexA, xfB, this.m_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
|
|
*/
|
|
Contact.prototype.update = function (listener) {
|
|
// Re-enable this contact.
|
|
this.m_enabledFlag = true;
|
|
var touching = false;
|
|
var wasTouching = this.m_touchingFlag;
|
|
var sensorA = this.m_fixtureA.isSensor();
|
|
var sensorB = this.m_fixtureB.isSensor();
|
|
var sensor = sensorA || sensorB;
|
|
var bodyA = this.m_fixtureA.getBody();
|
|
var bodyB = this.m_fixtureB.getBody();
|
|
var xfA = bodyA.getTransform();
|
|
var xfB = bodyB.getTransform();
|
|
var oldManifold;
|
|
// Is this contact a sensor?
|
|
if (sensor) {
|
|
var shapeA = this.m_fixtureA.getShape();
|
|
var shapeB = this.m_fixtureB.getShape();
|
|
touching = testOverlap(shapeA, this.m_indexA, shapeB, this.m_indexB, xfA, xfB);
|
|
// Sensors don't generate manifolds.
|
|
this.m_manifold.pointCount = 0;
|
|
}
|
|
else {
|
|
// TODO reuse manifold
|
|
oldManifold = this.m_manifold;
|
|
this.m_manifold = new Manifold();
|
|
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 (var i = 0; i < this.m_manifold.pointCount; ++i) {
|
|
var nmp = this.m_manifold.points[i];
|
|
nmp.normalImpulse = 0.0;
|
|
nmp.tangentImpulse = 0.0;
|
|
for (var j = 0; j < oldManifold.pointCount; ++j) {
|
|
var 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;
|
|
if (!wasTouching && touching && listener) {
|
|
listener.beginContact(this);
|
|
}
|
|
if (wasTouching && !touching && listener) {
|
|
listener.endContact(this);
|
|
}
|
|
if (!sensor && touching && listener) {
|
|
listener.preSolve(this, oldManifold);
|
|
}
|
|
};
|
|
Contact.prototype.solvePositionConstraint = function (step) {
|
|
return this._solvePositionConstraint(step);
|
|
};
|
|
Contact.prototype.solvePositionConstraintTOI = function (step, toiA, toiB) {
|
|
return this._solvePositionConstraint(step, toiA, toiB);
|
|
};
|
|
Contact.prototype._solvePositionConstraint = function (step, toiA, toiB) {
|
|
var toi = !!toiA && !!toiB;
|
|
var fixtureA = this.m_fixtureA;
|
|
var fixtureB = this.m_fixtureB;
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
bodyA.c_velocity;
|
|
bodyB.c_velocity;
|
|
var positionA = bodyA.c_position;
|
|
var positionB = bodyB.c_position;
|
|
var localCenterA = Vec2.clone(this.p_localCenterA);
|
|
var localCenterB = Vec2.clone(this.p_localCenterB);
|
|
var mA = 0.0;
|
|
var iA = 0.0;
|
|
if (!toi || (bodyA == toiA || bodyA == toiB)) {
|
|
mA = this.p_invMassA;
|
|
iA = this.p_invIA;
|
|
}
|
|
var mB = 0.0;
|
|
var iB = 0.0;
|
|
if (!toi || (bodyB == toiA || bodyB == toiB)) {
|
|
mB = this.p_invMassB;
|
|
iB = this.p_invIB;
|
|
}
|
|
var cA = Vec2.clone(positionA.c);
|
|
var aA = positionA.a;
|
|
var cB = Vec2.clone(positionB.c);
|
|
var aB = positionB.a;
|
|
var minSeparation = 0.0;
|
|
// Solve normal constraints
|
|
for (var j = 0; j < this.p_pointCount; ++j) {
|
|
var xfA = Transform.identity();
|
|
var xfB = Transform.identity();
|
|
xfA.q.set(aA);
|
|
xfB.q.set(aB);
|
|
xfA.p = Vec2.sub(cA, Rot.mulVec2(xfA.q, localCenterA));
|
|
xfB.p = Vec2.sub(cB, Rot.mulVec2(xfB.q, localCenterB));
|
|
// PositionSolverManifold
|
|
var normal = void 0;
|
|
var point = void 0;
|
|
var separation = void 0;
|
|
switch (this.p_type) {
|
|
case ManifoldType.e_circles: {
|
|
var pointA = Transform.mulVec2(xfA, this.p_localPoint);
|
|
var pointB = Transform.mulVec2(xfB, this.p_localPoints[0]);
|
|
normal = Vec2.sub(pointB, pointA);
|
|
normal.normalize();
|
|
point = Vec2.combine(0.5, pointA, 0.5, pointB);
|
|
separation = Vec2.dot(Vec2.sub(pointB, pointA), normal) - this.p_radiusA - this.p_radiusB;
|
|
break;
|
|
}
|
|
case ManifoldType.e_faceA: {
|
|
normal = Rot.mulVec2(xfA.q, this.p_localNormal);
|
|
var planePoint = Transform.mulVec2(xfA, this.p_localPoint);
|
|
var clipPoint = Transform.mulVec2(xfB, this.p_localPoints[j]);
|
|
separation = Vec2.dot(Vec2.sub(clipPoint, planePoint), normal) - this.p_radiusA - this.p_radiusB;
|
|
point = clipPoint;
|
|
break;
|
|
}
|
|
case ManifoldType.e_faceB: {
|
|
normal = Rot.mulVec2(xfB.q, this.p_localNormal);
|
|
var planePoint = Transform.mulVec2(xfB, this.p_localPoint);
|
|
var clipPoint = Transform.mulVec2(xfA, this.p_localPoints[j]);
|
|
separation = Vec2.dot(Vec2.sub(clipPoint, planePoint), normal) - this.p_radiusA - this.p_radiusB;
|
|
point = clipPoint;
|
|
// Ensure normal points from A to B
|
|
normal.mul(-1);
|
|
break;
|
|
}
|
|
}
|
|
var rA = Vec2.sub(point, cA);
|
|
var rB = Vec2.sub(point, cB);
|
|
// Track max constraint error.
|
|
minSeparation = math.min(minSeparation, separation);
|
|
var baumgarte = toi ? Settings.toiBaugarte : Settings.baumgarte;
|
|
var linearSlop = Settings.linearSlop;
|
|
var maxLinearCorrection = Settings.maxLinearCorrection;
|
|
// Prevent large corrections and allow slop.
|
|
var C = math.clamp(baumgarte * (separation + linearSlop), -maxLinearCorrection, 0.0);
|
|
// Compute the effective mass.
|
|
var rnA = Vec2.cross(rA, normal);
|
|
var rnB = Vec2.cross(rB, normal);
|
|
var K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
|
// Compute normal impulse
|
|
var impulse = K > 0.0 ? -C / K : 0.0;
|
|
var P = Vec2.mul(impulse, normal);
|
|
cA.subMul(mA, P);
|
|
aA -= iA * Vec2.cross(rA, P);
|
|
cB.addMul(mB, P);
|
|
aB += iB * Vec2.cross(rB, P);
|
|
}
|
|
positionA.c.set(cA);
|
|
positionA.a = aA;
|
|
positionB.c.set(cB);
|
|
positionB.a = aB;
|
|
return minSeparation;
|
|
};
|
|
Contact.prototype.initVelocityConstraint = function (step) {
|
|
var fixtureA = this.m_fixtureA;
|
|
var fixtureB = this.m_fixtureB;
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
var velocityA = bodyA.c_velocity;
|
|
var velocityB = bodyB.c_velocity;
|
|
var positionA = bodyA.c_position;
|
|
var positionB = bodyB.c_position;
|
|
var radiusA = this.p_radiusA;
|
|
var radiusB = this.p_radiusB;
|
|
var manifold = this.getManifold();
|
|
var mA = this.v_invMassA;
|
|
var mB = this.v_invMassB;
|
|
var iA = this.v_invIA;
|
|
var iB = this.v_invIB;
|
|
var localCenterA = Vec2.clone(this.p_localCenterA);
|
|
var localCenterB = Vec2.clone(this.p_localCenterB);
|
|
var cA = Vec2.clone(positionA.c);
|
|
var aA = positionA.a;
|
|
var vA = Vec2.clone(velocityA.v);
|
|
var wA = velocityA.w;
|
|
var cB = Vec2.clone(positionB.c);
|
|
var aB = positionB.a;
|
|
var vB = Vec2.clone(velocityB.v);
|
|
var wB = velocityB.w;
|
|
var xfA = Transform.identity();
|
|
var xfB = Transform.identity();
|
|
xfA.q.set(aA);
|
|
xfB.q.set(aB);
|
|
xfA.p.setCombine(1, cA, -1, Rot.mulVec2(xfA.q, localCenterA));
|
|
xfB.p.setCombine(1, cB, -1, Rot.mulVec2(xfB.q, localCenterB));
|
|
var worldManifold = manifold.getWorldManifold(null, xfA, radiusA, xfB, radiusB);
|
|
this.v_normal.set(worldManifold.normal);
|
|
for (var j = 0; j < this.v_pointCount; ++j) {
|
|
var vcp = this.v_points[j]; // VelocityConstraintPoint
|
|
vcp.rA.set(Vec2.sub(worldManifold.points[j], cA));
|
|
vcp.rB.set(Vec2.sub(worldManifold.points[j], cB));
|
|
var rnA = Vec2.cross(vcp.rA, this.v_normal);
|
|
var rnB = Vec2.cross(vcp.rB, this.v_normal);
|
|
var kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
|
vcp.normalMass = kNormal > 0.0 ? 1.0 / kNormal : 0.0;
|
|
var tangent = Vec2.cross(this.v_normal, 1.0);
|
|
var rtA = Vec2.cross(vcp.rA, tangent);
|
|
var rtB = Vec2.cross(vcp.rB, tangent);
|
|
var 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;
|
|
var vRel = Vec2.dot(this.v_normal, vB)
|
|
+ Vec2.dot(this.v_normal, Vec2.cross(wB, vcp.rB))
|
|
- Vec2.dot(this.v_normal, vA)
|
|
- Vec2.dot(this.v_normal, Vec2.cross(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) {
|
|
var vcp1 = this.v_points[0]; // VelocityConstraintPoint
|
|
var vcp2 = this.v_points[1]; // VelocityConstraintPoint
|
|
var rn1A = Vec2.cross(vcp1.rA, this.v_normal);
|
|
var rn1B = Vec2.cross(vcp1.rB, this.v_normal);
|
|
var rn2A = Vec2.cross(vcp2.rA, this.v_normal);
|
|
var rn2B = Vec2.cross(vcp2.rB, this.v_normal);
|
|
var k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
|
|
var k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
|
|
var k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
|
|
// Ensure a reasonable condition number.
|
|
var k_maxConditionNumber = 1000.0;
|
|
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
|
|
// K is safe to invert.
|
|
this.v_K.ex.set(k11, k12);
|
|
this.v_K.ey.set(k12, k22);
|
|
this.v_normalMass.set(this.v_K.getInverse());
|
|
}
|
|
else {
|
|
// The constraints are redundant, just use one.
|
|
// TODO_ERIN use deepest?
|
|
this.v_pointCount = 1;
|
|
}
|
|
}
|
|
positionA.c.set(cA);
|
|
positionA.a = aA;
|
|
velocityA.v.set(vA);
|
|
velocityA.w = wA;
|
|
positionB.c.set(cB);
|
|
positionB.a = aB;
|
|
velocityB.v.set(vB);
|
|
velocityB.w = wB;
|
|
};
|
|
Contact.prototype.warmStartConstraint = function (step) {
|
|
var fixtureA = this.m_fixtureA;
|
|
var fixtureB = this.m_fixtureB;
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
var velocityA = bodyA.c_velocity;
|
|
var velocityB = bodyB.c_velocity;
|
|
bodyA.c_position;
|
|
bodyB.c_position;
|
|
var mA = this.v_invMassA;
|
|
var iA = this.v_invIA;
|
|
var mB = this.v_invMassB;
|
|
var iB = this.v_invIB;
|
|
var vA = Vec2.clone(velocityA.v);
|
|
var wA = velocityA.w;
|
|
var vB = Vec2.clone(velocityB.v);
|
|
var wB = velocityB.w;
|
|
var normal = this.v_normal;
|
|
var tangent = Vec2.cross(normal, 1.0);
|
|
for (var j = 0; j < this.v_pointCount; ++j) {
|
|
var vcp = this.v_points[j]; // VelocityConstraintPoint
|
|
var P = Vec2.combine(vcp.normalImpulse, normal, vcp.tangentImpulse, tangent);
|
|
wA -= iA * Vec2.cross(vcp.rA, P);
|
|
vA.subMul(mA, P);
|
|
wB += iB * Vec2.cross(vcp.rB, P);
|
|
vB.addMul(mB, P);
|
|
}
|
|
velocityA.v.set(vA);
|
|
velocityA.w = wA;
|
|
velocityB.v.set(vB);
|
|
velocityB.w = wB;
|
|
};
|
|
Contact.prototype.storeConstraintImpulses = function (step) {
|
|
var manifold = this.m_manifold;
|
|
for (var 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;
|
|
}
|
|
};
|
|
Contact.prototype.solveVelocityConstraint = function (step) {
|
|
var bodyA = this.m_fixtureA.m_body;
|
|
var bodyB = this.m_fixtureB.m_body;
|
|
var velocityA = bodyA.c_velocity;
|
|
bodyA.c_position;
|
|
var velocityB = bodyB.c_velocity;
|
|
bodyB.c_position;
|
|
var mA = this.v_invMassA;
|
|
var iA = this.v_invIA;
|
|
var mB = this.v_invMassB;
|
|
var iB = this.v_invIB;
|
|
var vA = Vec2.clone(velocityA.v);
|
|
var wA = velocityA.w;
|
|
var vB = Vec2.clone(velocityB.v);
|
|
var wB = velocityB.w;
|
|
var normal = this.v_normal;
|
|
var tangent = Vec2.cross(normal, 1.0);
|
|
var friction = this.v_friction;
|
|
// Solve tangent constraints first because non-penetration is more important
|
|
// than friction.
|
|
for (var j = 0; j < this.v_pointCount; ++j) {
|
|
var vcp = this.v_points[j]; // VelocityConstraintPoint
|
|
// Relative velocity at contact
|
|
var dv = Vec2.zero();
|
|
dv.addCombine(1, vB, 1, Vec2.cross(wB, vcp.rB));
|
|
dv.subCombine(1, vA, 1, Vec2.cross(wA, vcp.rA));
|
|
// Compute tangent force
|
|
var vt = Vec2.dot(dv, tangent) - this.v_tangentSpeed;
|
|
var lambda = vcp.tangentMass * (-vt);
|
|
// Clamp the accumulated force
|
|
var maxFriction = friction * vcp.normalImpulse;
|
|
var newImpulse = math.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
|
|
lambda = newImpulse - vcp.tangentImpulse;
|
|
vcp.tangentImpulse = newImpulse;
|
|
// Apply contact impulse
|
|
var P = Vec2.mul(lambda, tangent);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * Vec2.cross(vcp.rA, P);
|
|
vB.addMul(mB, P);
|
|
wB += iB * Vec2.cross(vcp.rB, P);
|
|
}
|
|
// Solve normal constraints
|
|
if (this.v_pointCount == 1 || step.blockSolve == false) {
|
|
for (var i = 0; i < this.v_pointCount; ++i) {
|
|
var vcp = this.v_points[i]; // VelocityConstraintPoint
|
|
// Relative velocity at contact
|
|
var dv = Vec2.zero();
|
|
dv.addCombine(1, vB, 1, Vec2.cross(wB, vcp.rB));
|
|
dv.subCombine(1, vA, 1, Vec2.cross(wA, vcp.rA));
|
|
// Compute normal impulse
|
|
var vn = Vec2.dot(dv, normal);
|
|
var lambda = -vcp.normalMass * (vn - vcp.velocityBias);
|
|
// Clamp the accumulated impulse
|
|
var newImpulse = math.max(vcp.normalImpulse + lambda, 0.0);
|
|
lambda = newImpulse - vcp.normalImpulse;
|
|
vcp.normalImpulse = newImpulse;
|
|
// Apply contact impulse
|
|
var P = Vec2.mul(lambda, normal);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * Vec2.cross(vcp.rA, P);
|
|
vB.addMul(mB, P);
|
|
wB += iB * Vec2.cross(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, , 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;
|
|
var vcp1 = this.v_points[0]; // VelocityConstraintPoint
|
|
var vcp2 = this.v_points[1]; // VelocityConstraintPoint
|
|
var a = Vec2.neo(vcp1.normalImpulse, vcp2.normalImpulse);
|
|
// Relative velocity at contact
|
|
var dv1 = Vec2.zero().add(vB).add(Vec2.cross(wB, vcp1.rB)).sub(vA).sub(Vec2.cross(wA, vcp1.rA));
|
|
var dv2 = Vec2.zero().add(vB).add(Vec2.cross(wB, vcp2.rB)).sub(vA).sub(Vec2.cross(wA, vcp2.rA));
|
|
// Compute normal velocity
|
|
var vn1 = Vec2.dot(dv1, normal);
|
|
var vn2 = Vec2.dot(dv2, normal);
|
|
var b = Vec2.neo(vn1 - vcp1.velocityBias, vn2 - vcp2.velocityBias);
|
|
// Compute b'
|
|
b.sub(Mat22.mulVec2(this.v_K, a));
|
|
// NOT_USED(k_errorTol);
|
|
while (true) {
|
|
//
|
|
// Case 1: vn = 0
|
|
//
|
|
// 0 = A * x + b'
|
|
//
|
|
// Solve for x:
|
|
//
|
|
// x = - inv(A) * b'
|
|
//
|
|
var x = Mat22.mulVec2(this.v_normalMass, b).neg();
|
|
if (x.x >= 0.0 && x.y >= 0.0) {
|
|
// Get the incremental impulse
|
|
var d = Vec2.sub(x, a);
|
|
// Apply incremental impulse
|
|
var P1 = Vec2.mul(d.x, normal);
|
|
var P2 = Vec2.mul(d.y, normal);
|
|
vA.subCombine(mA, P1, mA, P2);
|
|
wA -= iA * (Vec2.cross(vcp1.rA, P1) + Vec2.cross(vcp2.rA, P2));
|
|
vB.addCombine(mB, P1, mB, P2);
|
|
wB += iB * (Vec2.cross(vcp1.rB, P1) + Vec2.cross(vcp2.rB, P2));
|
|
// Accumulate
|
|
vcp1.normalImpulse = x.x;
|
|
vcp2.normalImpulse = x.y;
|
|
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
|
|
var d = Vec2.sub(x, a);
|
|
// Apply incremental impulse
|
|
var P1 = Vec2.mul(d.x, normal);
|
|
var P2 = Vec2.mul(d.y, normal);
|
|
vA.subCombine(mA, P1, mA, P2);
|
|
wA -= iA * (Vec2.cross(vcp1.rA, P1) + Vec2.cross(vcp2.rA, P2));
|
|
vB.addCombine(mB, P1, mB, P2);
|
|
wB += iB * (Vec2.cross(vcp1.rB, P1) + Vec2.cross(vcp2.rB, P2));
|
|
// Accumulate
|
|
vcp1.normalImpulse = x.x;
|
|
vcp2.normalImpulse = x.y;
|
|
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
|
|
var d = Vec2.sub(x, a);
|
|
// Apply incremental impulse
|
|
var P1 = Vec2.mul(d.x, normal);
|
|
var P2 = Vec2.mul(d.y, normal);
|
|
vA.subCombine(mA, P1, mA, P2);
|
|
wA -= iA * (Vec2.cross(vcp1.rA, P1) + Vec2.cross(vcp2.rA, P2));
|
|
vB.addCombine(mB, P1, mB, P2);
|
|
wB += iB * (Vec2.cross(vcp1.rB, P1) + Vec2.cross(vcp2.rB, P2));
|
|
// Accumulate
|
|
vcp1.normalImpulse = x.x;
|
|
vcp2.normalImpulse = x.y;
|
|
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
|
|
var d = Vec2.sub(x, a);
|
|
// Apply incremental impulse
|
|
var P1 = Vec2.mul(d.x, normal);
|
|
var P2 = Vec2.mul(d.y, normal);
|
|
vA.subCombine(mA, P1, mA, P2);
|
|
wA -= iA * (Vec2.cross(vcp1.rA, P1) + Vec2.cross(vcp2.rA, P2));
|
|
vB.addCombine(mB, P1, mB, P2);
|
|
wB += iB * (Vec2.cross(vcp1.rB, P1) + Vec2.cross(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;
|
|
}
|
|
}
|
|
velocityA.v.set(vA);
|
|
velocityA.w = wA;
|
|
velocityB.v.set(vB);
|
|
velocityB.w = wB;
|
|
};
|
|
/**
|
|
* @internal
|
|
*/
|
|
Contact.addType = function (type1, type2, callback) {
|
|
s_registers[type1] = s_registers[type1] || {};
|
|
s_registers[type1][type2] = callback;
|
|
};
|
|
/**
|
|
* @internal
|
|
*/
|
|
Contact.create = function (fixtureA, indexA, fixtureB, indexB) {
|
|
var typeA = fixtureA.getType();
|
|
var typeB = fixtureB.getType();
|
|
// TODO: pool contacts
|
|
var contact;
|
|
var evaluateFcn;
|
|
if (evaluateFcn = s_registers[typeA] && s_registers[typeA][typeB]) {
|
|
contact = new Contact(fixtureA, indexA, fixtureB, indexB, evaluateFcn);
|
|
}
|
|
else if (evaluateFcn = s_registers[typeB] && s_registers[typeB][typeA]) {
|
|
contact = new Contact(fixtureB, indexB, fixtureA, indexA, evaluateFcn);
|
|
}
|
|
else {
|
|
return null;
|
|
}
|
|
// Contact creation may swap fixtures.
|
|
fixtureA = contact.getFixtureA();
|
|
fixtureB = contact.getFixtureB();
|
|
indexA = contact.getChildIndexA();
|
|
indexB = contact.getChildIndexB();
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
// 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
|
|
*/
|
|
Contact.destroy = function (contact, listener) {
|
|
var fixtureA = contact.m_fixtureA;
|
|
var fixtureB = contact.m_fixtureB;
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
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.isSensor() == false
|
|
&& fixtureB.isSensor() == false) {
|
|
bodyA.setAwake(true);
|
|
bodyB.setAwake(true);
|
|
}
|
|
fixtureA.getType();
|
|
fixtureB.getType();
|
|
// const destroyFcn = s_registers[typeA][typeB].destroyFcn;
|
|
// if (typeof destroyFcn === 'function') {
|
|
// destroyFcn(contact);
|
|
// }
|
|
};
|
|
return Contact;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A joint edge is used to connect bodies and joints together in a joint graph
|
|
* where each body is a node and each joint is an edge. A joint edge belongs to
|
|
* a doubly linked list maintained in each attached body. Each joint has two
|
|
* joint nodes, one for each attached body.
|
|
*/
|
|
var JointEdge = /** @class */ (function () {
|
|
function JointEdge() {
|
|
/**
|
|
* provides quick access to the other body attached.
|
|
*/
|
|
this.other = null;
|
|
/**
|
|
* the joint
|
|
*/
|
|
this.joint = null;
|
|
/**
|
|
* prev the previous joint edge in the body's joint list
|
|
*/
|
|
this.prev = null;
|
|
/**
|
|
* the next joint edge in the body's joint list
|
|
*/
|
|
this.next = null;
|
|
}
|
|
return JointEdge;
|
|
}());
|
|
/**
|
|
* The base joint class. Joints are used to constraint two bodies together in
|
|
* various fashions. Some joints also feature limits and motors.
|
|
*/
|
|
var Joint = /** @class */ (function () {
|
|
function Joint(def, bodyA, bodyB) {
|
|
/** @internal */ this.m_type = 'unknown-joint';
|
|
/** @internal */ this.m_prev = null;
|
|
/** @internal */ this.m_next = null;
|
|
/** @internal */ this.m_edgeA = new JointEdge();
|
|
/** @internal */ this.m_edgeB = new JointEdge();
|
|
/** @internal */ this.m_islandFlag = false;
|
|
bodyA = 'bodyA' in def ? def.bodyA : bodyA;
|
|
bodyB = 'bodyB' in def ? def.bodyB : bodyB;
|
|
this.m_bodyA = bodyA;
|
|
this.m_bodyB = bodyB;
|
|
this.m_collideConnected = !!def.collideConnected;
|
|
this.m_userData = def.userData;
|
|
}
|
|
/**
|
|
* Short-cut function to determine if either body is inactive.
|
|
*/
|
|
Joint.prototype.isActive = function () {
|
|
return this.m_bodyA.isActive() && this.m_bodyB.isActive();
|
|
};
|
|
/**
|
|
* Get the type of the concrete joint.
|
|
*/
|
|
Joint.prototype.getType = function () {
|
|
return this.m_type;
|
|
};
|
|
/**
|
|
* Get the first body attached to this joint.
|
|
*/
|
|
Joint.prototype.getBodyA = function () {
|
|
return this.m_bodyA;
|
|
};
|
|
/**
|
|
* Get the second body attached to this joint.
|
|
*/
|
|
Joint.prototype.getBodyB = function () {
|
|
return this.m_bodyB;
|
|
};
|
|
/**
|
|
* Get the next joint the world joint list.
|
|
*/
|
|
Joint.prototype.getNext = function () {
|
|
return this.m_next;
|
|
};
|
|
Joint.prototype.getUserData = function () {
|
|
return this.m_userData;
|
|
};
|
|
Joint.prototype.setUserData = function (data) {
|
|
this.m_userData = data;
|
|
};
|
|
/**
|
|
* Get collide connected. Note: modifying the collide connect flag won't work
|
|
* correctly because the flag is only checked when fixture AABBs begin to
|
|
* overlap.
|
|
*/
|
|
Joint.prototype.getCollideConnected = function () {
|
|
return this.m_collideConnected;
|
|
};
|
|
/**
|
|
* Shift the origin for any points stored in world coordinates.
|
|
*/
|
|
Joint.prototype.shiftOrigin = function (newOrigin) { };
|
|
Joint.TYPES = {};
|
|
/** @internal */
|
|
Joint._deserialize = function (data, context, restore) {
|
|
var clazz = Joint.TYPES[data.type];
|
|
return clazz && restore(clazz, data);
|
|
};
|
|
return Joint;
|
|
}());
|
|
|
|
var now = function () {
|
|
return Date.now();
|
|
};
|
|
var diff = function (time) {
|
|
return Date.now() - time;
|
|
};
|
|
var Timer = {
|
|
now: now,
|
|
diff: diff,
|
|
};
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* Input parameters for TimeOfImpact.
|
|
*/
|
|
var TOIInput = /** @class */ (function () {
|
|
function TOIInput() {
|
|
this.proxyA = new DistanceProxy();
|
|
this.proxyB = new DistanceProxy();
|
|
this.sweepA = new Sweep();
|
|
this.sweepB = new Sweep();
|
|
}
|
|
return TOIInput;
|
|
}());
|
|
var TOIOutputState;
|
|
(function (TOIOutputState) {
|
|
TOIOutputState[TOIOutputState["e_unknown"] = 0] = "e_unknown";
|
|
TOIOutputState[TOIOutputState["e_failed"] = 1] = "e_failed";
|
|
TOIOutputState[TOIOutputState["e_overlapped"] = 2] = "e_overlapped";
|
|
TOIOutputState[TOIOutputState["e_touching"] = 3] = "e_touching";
|
|
TOIOutputState[TOIOutputState["e_separated"] = 4] = "e_separated";
|
|
})(TOIOutputState || (TOIOutputState = {}));
|
|
/**
|
|
* Output parameters for TimeOfImpact.
|
|
*/
|
|
var TOIOutput = /** @class */ (function () {
|
|
function TOIOutput() {
|
|
}
|
|
return TOIOutput;
|
|
}());
|
|
stats.toiTime = 0;
|
|
stats.toiMaxTime = 0;
|
|
stats.toiCalls = 0;
|
|
stats.toiIters = 0;
|
|
stats.toiMaxIters = 0;
|
|
stats.toiRootIters = 0;
|
|
stats.toiMaxRootIters = 0;
|
|
/**
|
|
* Compute the upper bound on time before two shapes penetrate. Time is
|
|
* represented as a fraction between [0,tMax]. This uses a swept separating axis
|
|
* and may miss some intermediate, non-tunneling collision. If you change the
|
|
* time interval, you should call this function again.
|
|
*
|
|
* Note: use Distance to compute the contact point and normal at the time of
|
|
* impact.
|
|
*
|
|
* CCD via the local separating axis method. This seeks progression by computing
|
|
* the largest time at which separation is maintained.
|
|
*/
|
|
function TimeOfImpact(output, input) {
|
|
var timer = Timer.now();
|
|
++stats.toiCalls;
|
|
output.state = TOIOutputState.e_unknown;
|
|
output.t = input.tMax;
|
|
var proxyA = input.proxyA; // DistanceProxy
|
|
var proxyB = input.proxyB; // DistanceProxy
|
|
var sweepA = input.sweepA; // Sweep
|
|
var sweepB = input.sweepB; // Sweep
|
|
// Large rotations can make the root finder fail, so we normalize the
|
|
// sweep angles.
|
|
sweepA.normalize();
|
|
sweepB.normalize();
|
|
var tMax = input.tMax;
|
|
var totalRadius = proxyA.m_radius + proxyB.m_radius;
|
|
var target = math.max(Settings.linearSlop, totalRadius - 3.0 * Settings.linearSlop);
|
|
var tolerance = 0.25 * Settings.linearSlop;
|
|
var t1 = 0.0;
|
|
var k_maxIterations = Settings.maxTOIIterations;
|
|
var iter = 0;
|
|
// Prepare input for distance query.
|
|
var cache = new SimplexCache();
|
|
var distanceInput = new DistanceInput();
|
|
distanceInput.proxyA = input.proxyA;
|
|
distanceInput.proxyB = input.proxyB;
|
|
distanceInput.useRadii = false;
|
|
// The outer loop progressively attempts to compute new separating axes.
|
|
// This loop terminates when an axis is repeated (no progress is made).
|
|
while (true) {
|
|
var xfA = Transform.identity();
|
|
var xfB = Transform.identity();
|
|
sweepA.getTransform(xfA, t1);
|
|
sweepB.getTransform(xfB, t1);
|
|
// Get the distance between shapes. We can also use the results
|
|
// to get a separating axis.
|
|
distanceInput.transformA = xfA;
|
|
distanceInput.transformB = xfB;
|
|
var distanceOutput = new DistanceOutput();
|
|
Distance(distanceOutput, cache, distanceInput);
|
|
// If the shapes are overlapped, we give up on continuous collision.
|
|
if (distanceOutput.distance <= 0.0) {
|
|
// Failure!
|
|
output.state = TOIOutputState.e_overlapped;
|
|
output.t = 0.0;
|
|
break;
|
|
}
|
|
if (distanceOutput.distance < target + tolerance) {
|
|
// Victory!
|
|
output.state = TOIOutputState.e_touching;
|
|
output.t = t1;
|
|
break;
|
|
}
|
|
// Initialize the separating axis.
|
|
var fcn = new SeparationFunction();
|
|
fcn.initialize(cache, proxyA, sweepA, proxyB, sweepB, t1);
|
|
// if (false) {
|
|
// // Dump the curve seen by the root finder
|
|
// const N = 100;
|
|
// const dx = 1.0 / N;
|
|
// const xs = []; // [ N + 1 ];
|
|
// const fs = []; // [ N + 1 ];
|
|
// const x = 0.0;
|
|
// for (const i = 0; i <= N; ++i) {
|
|
// sweepA.getTransform(xfA, x);
|
|
// sweepB.getTransform(xfB, x);
|
|
// const f = fcn.evaluate(xfA, xfB) - target;
|
|
// printf("%g %g\n", x, f);
|
|
// xs[i] = x;
|
|
// fs[i] = f;
|
|
// x += dx;
|
|
// }
|
|
// }
|
|
// Compute the TOI on the separating axis. We do this by successively
|
|
// resolving the deepest point. This loop is bounded by the number of
|
|
// vertices.
|
|
var done = false;
|
|
var t2 = tMax;
|
|
var pushBackIter = 0;
|
|
while (true) {
|
|
// Find the deepest point at t2. Store the witness point indices.
|
|
var s2 = fcn.findMinSeparation(t2);
|
|
// const indexA = fcn.indexA;
|
|
// const indexB = fcn.indexB;
|
|
// Is the final configuration separated?
|
|
if (s2 > target + tolerance) {
|
|
// Victory!
|
|
output.state = TOIOutputState.e_separated;
|
|
output.t = tMax;
|
|
done = true;
|
|
break;
|
|
}
|
|
// Has the separation reached tolerance?
|
|
if (s2 > target - tolerance) {
|
|
// Advance the sweeps
|
|
t1 = t2;
|
|
break;
|
|
}
|
|
// Compute the initial separation of the witness points.
|
|
var s1 = fcn.evaluate(t1);
|
|
// const indexA = fcn.indexA;
|
|
// const indexB = fcn.indexB;
|
|
// Check for initial overlap. This might happen if the root finder
|
|
// runs out of iterations.
|
|
if (s1 < target - tolerance) {
|
|
output.state = TOIOutputState.e_failed;
|
|
output.t = t1;
|
|
done = true;
|
|
break;
|
|
}
|
|
// Check for touching
|
|
if (s1 <= target + tolerance) {
|
|
// Victory! t1 should hold the TOI (could be 0.0).
|
|
output.state = TOIOutputState.e_touching;
|
|
output.t = t1;
|
|
done = true;
|
|
break;
|
|
}
|
|
// Compute 1D root of: f(x) - target = 0
|
|
var rootIterCount = 0;
|
|
var a1 = t1;
|
|
var a2 = t2;
|
|
while (true) {
|
|
// Use a mix of the secant rule and bisection.
|
|
var t = void 0;
|
|
if (rootIterCount & 1) {
|
|
// Secant rule to improve convergence.
|
|
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
|
|
}
|
|
else {
|
|
// Bisection to guarantee progress.
|
|
t = 0.5 * (a1 + a2);
|
|
}
|
|
++rootIterCount;
|
|
++stats.toiRootIters;
|
|
var s = fcn.evaluate(t);
|
|
fcn.indexA;
|
|
fcn.indexB;
|
|
if (math.abs(s - target) < tolerance) {
|
|
// t2 holds a tentative value for t1
|
|
t2 = t;
|
|
break;
|
|
}
|
|
// Ensure we continue to bracket the root.
|
|
if (s > target) {
|
|
a1 = t;
|
|
s1 = s;
|
|
}
|
|
else {
|
|
a2 = t;
|
|
s2 = s;
|
|
}
|
|
if (rootIterCount === 50) {
|
|
break;
|
|
}
|
|
}
|
|
stats.toiMaxRootIters = math.max(stats.toiMaxRootIters, rootIterCount);
|
|
++pushBackIter;
|
|
if (pushBackIter === Settings.maxPolygonVertices) {
|
|
break;
|
|
}
|
|
}
|
|
++iter;
|
|
++stats.toiIters;
|
|
if (done) {
|
|
break;
|
|
}
|
|
if (iter === k_maxIterations) {
|
|
// Root finder got stuck. Semi-victory.
|
|
output.state = TOIOutputState.e_failed;
|
|
output.t = t1;
|
|
break;
|
|
}
|
|
}
|
|
stats.toiMaxIters = math.max(stats.toiMaxIters, iter);
|
|
var time = Timer.diff(timer);
|
|
stats.toiMaxTime = math.max(stats.toiMaxTime, time);
|
|
stats.toiTime += time;
|
|
}
|
|
var SeparationFunctionType;
|
|
(function (SeparationFunctionType) {
|
|
SeparationFunctionType[SeparationFunctionType["e_points"] = 1] = "e_points";
|
|
SeparationFunctionType[SeparationFunctionType["e_faceA"] = 2] = "e_faceA";
|
|
SeparationFunctionType[SeparationFunctionType["e_faceB"] = 3] = "e_faceB";
|
|
})(SeparationFunctionType || (SeparationFunctionType = {}));
|
|
var SeparationFunction = /** @class */ (function () {
|
|
function SeparationFunction() {
|
|
this.m_proxyA = new DistanceProxy();
|
|
this.m_proxyB = new DistanceProxy();
|
|
this.m_localPoint = Vec2.zero();
|
|
this.m_axis = Vec2.zero();
|
|
}
|
|
// TODO_ERIN might not need to return the separation
|
|
SeparationFunction.prototype.initialize = function (cache, proxyA, sweepA, proxyB, sweepB, t1) {
|
|
this.m_proxyA = proxyA;
|
|
this.m_proxyB = proxyB;
|
|
var count = cache.count;
|
|
this.m_sweepA = sweepA;
|
|
this.m_sweepB = sweepB;
|
|
var xfA = Transform.identity();
|
|
var xfB = Transform.identity();
|
|
this.m_sweepA.getTransform(xfA, t1);
|
|
this.m_sweepB.getTransform(xfB, t1);
|
|
if (count === 1) {
|
|
this.m_type = SeparationFunctionType.e_points;
|
|
var localPointA = this.m_proxyA.getVertex(cache.indexA[0]);
|
|
var localPointB = this.m_proxyB.getVertex(cache.indexB[0]);
|
|
var pointA = Transform.mulVec2(xfA, localPointA);
|
|
var pointB = Transform.mulVec2(xfB, localPointB);
|
|
this.m_axis.setCombine(1, pointB, -1, pointA);
|
|
var s = this.m_axis.normalize();
|
|
return s;
|
|
}
|
|
else if (cache.indexA[0] === cache.indexA[1]) {
|
|
// Two points on B and one on A.
|
|
this.m_type = SeparationFunctionType.e_faceB;
|
|
var localPointB1 = proxyB.getVertex(cache.indexB[0]);
|
|
var localPointB2 = proxyB.getVertex(cache.indexB[1]);
|
|
this.m_axis = Vec2.cross(Vec2.sub(localPointB2, localPointB1), 1.0);
|
|
this.m_axis.normalize();
|
|
var normal = Rot.mulVec2(xfB.q, this.m_axis);
|
|
this.m_localPoint = Vec2.mid(localPointB1, localPointB2);
|
|
var pointB = Transform.mulVec2(xfB, this.m_localPoint);
|
|
var localPointA = proxyA.getVertex(cache.indexA[0]);
|
|
var pointA = Transform.mulVec2(xfA, localPointA);
|
|
var s = Vec2.dot(pointA, normal) - Vec2.dot(pointB, normal);
|
|
if (s < 0.0) {
|
|
this.m_axis = Vec2.neg(this.m_axis);
|
|
s = -s;
|
|
}
|
|
return s;
|
|
}
|
|
else {
|
|
// Two points on A and one or two points on B.
|
|
this.m_type = SeparationFunctionType.e_faceA;
|
|
var localPointA1 = this.m_proxyA.getVertex(cache.indexA[0]);
|
|
var localPointA2 = this.m_proxyA.getVertex(cache.indexA[1]);
|
|
this.m_axis = Vec2.cross(Vec2.sub(localPointA2, localPointA1), 1.0);
|
|
this.m_axis.normalize();
|
|
var normal = Rot.mulVec2(xfA.q, this.m_axis);
|
|
this.m_localPoint = Vec2.mid(localPointA1, localPointA2);
|
|
var pointA = Transform.mulVec2(xfA, this.m_localPoint);
|
|
var localPointB = this.m_proxyB.getVertex(cache.indexB[0]);
|
|
var pointB = Transform.mulVec2(xfB, localPointB);
|
|
var s = Vec2.dot(pointB, normal) - Vec2.dot(pointA, normal);
|
|
if (s < 0.0) {
|
|
this.m_axis = Vec2.neg(this.m_axis);
|
|
s = -s;
|
|
}
|
|
return s;
|
|
}
|
|
};
|
|
SeparationFunction.prototype.compute = function (find, t) {
|
|
// It was findMinSeparation and evaluate
|
|
var xfA = Transform.identity();
|
|
var xfB = Transform.identity();
|
|
this.m_sweepA.getTransform(xfA, t);
|
|
this.m_sweepB.getTransform(xfB, t);
|
|
switch (this.m_type) {
|
|
case SeparationFunctionType.e_points: {
|
|
if (find) {
|
|
var axisA = Rot.mulTVec2(xfA.q, this.m_axis);
|
|
var axisB = Rot.mulTVec2(xfB.q, Vec2.neg(this.m_axis));
|
|
this.indexA = this.m_proxyA.getSupport(axisA);
|
|
this.indexB = this.m_proxyB.getSupport(axisB);
|
|
}
|
|
var localPointA = this.m_proxyA.getVertex(this.indexA);
|
|
var localPointB = this.m_proxyB.getVertex(this.indexB);
|
|
var pointA = Transform.mulVec2(xfA, localPointA);
|
|
var pointB = Transform.mulVec2(xfB, localPointB);
|
|
var sep = Vec2.dot(pointB, this.m_axis) - Vec2.dot(pointA, this.m_axis);
|
|
return sep;
|
|
}
|
|
case SeparationFunctionType.e_faceA: {
|
|
var normal = Rot.mulVec2(xfA.q, this.m_axis);
|
|
var pointA = Transform.mulVec2(xfA, this.m_localPoint);
|
|
if (find) {
|
|
var axisB = Rot.mulTVec2(xfB.q, Vec2.neg(normal));
|
|
this.indexA = -1;
|
|
this.indexB = this.m_proxyB.getSupport(axisB);
|
|
}
|
|
var localPointB = this.m_proxyB.getVertex(this.indexB);
|
|
var pointB = Transform.mulVec2(xfB, localPointB);
|
|
var sep = Vec2.dot(pointB, normal) - Vec2.dot(pointA, normal);
|
|
return sep;
|
|
}
|
|
case SeparationFunctionType.e_faceB: {
|
|
var normal = Rot.mulVec2(xfB.q, this.m_axis);
|
|
var pointB = Transform.mulVec2(xfB, this.m_localPoint);
|
|
if (find) {
|
|
var axisA = Rot.mulTVec2(xfA.q, Vec2.neg(normal));
|
|
this.indexB = -1;
|
|
this.indexA = this.m_proxyA.getSupport(axisA);
|
|
}
|
|
var localPointA = this.m_proxyA.getVertex(this.indexA);
|
|
var pointA = Transform.mulVec2(xfA, localPointA);
|
|
var sep = Vec2.dot(pointA, normal) - Vec2.dot(pointB, normal);
|
|
return sep;
|
|
}
|
|
default:
|
|
if (find) {
|
|
this.indexA = -1;
|
|
this.indexB = -1;
|
|
}
|
|
return 0.0;
|
|
}
|
|
};
|
|
SeparationFunction.prototype.findMinSeparation = function (t) {
|
|
return this.compute(true, t);
|
|
};
|
|
SeparationFunction.prototype.evaluate = function (t) {
|
|
return this.compute(false, t);
|
|
};
|
|
return SeparationFunction;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var TimeStep = /** @class */ (function () {
|
|
function TimeStep() {
|
|
/** time step */
|
|
this.dt = 0;
|
|
/** inverse time step (0 if dt == 0) */
|
|
this.inv_dt = 0;
|
|
this.velocityIterations = 0;
|
|
this.positionIterations = 0;
|
|
this.warmStarting = false;
|
|
this.blockSolve = true;
|
|
/** timestep ratio for variable timestep */
|
|
this.inv_dt0 = 0.0;
|
|
/** dt * inv_dt0 */
|
|
this.dtRatio = 1;
|
|
}
|
|
TimeStep.prototype.reset = function (dt) {
|
|
if (this.dt > 0.0) {
|
|
this.inv_dt0 = this.inv_dt;
|
|
}
|
|
this.dt = dt;
|
|
this.inv_dt = dt == 0 ? 0 : 1 / dt;
|
|
this.dtRatio = dt * this.inv_dt0;
|
|
};
|
|
return TimeStep;
|
|
}());
|
|
// reuse
|
|
var s_subStep = new TimeStep();
|
|
/**
|
|
* Contact impulses for reporting. Impulses are used instead of forces because
|
|
* sub-step forces may approach infinity for rigid body collisions. These match
|
|
* up one-to-one with the contact points in Manifold.
|
|
*/
|
|
var ContactImpulse = /** @class */ (function () {
|
|
function ContactImpulse(contact) {
|
|
this.contact = contact;
|
|
this.normals = [];
|
|
this.tangents = [];
|
|
}
|
|
Object.defineProperty(ContactImpulse.prototype, "normalImpulses", {
|
|
get: function () {
|
|
var contact = this.contact;
|
|
var normals = this.normals;
|
|
normals.length = 0;
|
|
for (var p = 0; p < contact.v_points.length; ++p) {
|
|
normals.push(contact.v_points[p].normalImpulse);
|
|
}
|
|
return normals;
|
|
},
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
Object.defineProperty(ContactImpulse.prototype, "tangentImpulses", {
|
|
get: function () {
|
|
var contact = this.contact;
|
|
var tangents = this.tangents;
|
|
tangents.length = 0;
|
|
for (var p = 0; p < contact.v_points.length; ++p) {
|
|
tangents.push(contact.v_points[p].tangentImpulse);
|
|
}
|
|
return tangents;
|
|
},
|
|
enumerable: false,
|
|
configurable: true
|
|
});
|
|
return ContactImpulse;
|
|
}());
|
|
/**
|
|
* Finds and solves islands. An island is a connected subset of the world.
|
|
*/
|
|
var Solver = /** @class */ (function () {
|
|
function Solver(world) {
|
|
this.m_world = world;
|
|
this.m_stack = [];
|
|
this.m_bodies = [];
|
|
this.m_contacts = [];
|
|
this.m_joints = [];
|
|
}
|
|
Solver.prototype.clear = function () {
|
|
this.m_stack.length = 0;
|
|
this.m_bodies.length = 0;
|
|
this.m_contacts.length = 0;
|
|
this.m_joints.length = 0;
|
|
};
|
|
Solver.prototype.addBody = function (body) {
|
|
this.m_bodies.push(body);
|
|
// why?
|
|
// body.c_position.c.setZero();
|
|
// body.c_position.a = 0;
|
|
// body.c_velocity.v.setZero();
|
|
// body.c_velocity.w = 0;
|
|
};
|
|
Solver.prototype.addContact = function (contact) {
|
|
this.m_contacts.push(contact);
|
|
};
|
|
Solver.prototype.addJoint = function (joint) {
|
|
this.m_joints.push(joint);
|
|
};
|
|
Solver.prototype.solveWorld = function (step) {
|
|
var world = this.m_world;
|
|
// Clear all the island flags.
|
|
for (var b = world.m_bodyList; b; b = b.m_next) {
|
|
b.m_islandFlag = false;
|
|
}
|
|
for (var c = world.m_contactList; c; c = c.m_next) {
|
|
c.m_islandFlag = false;
|
|
}
|
|
for (var j = world.m_jointList; j; j = j.m_next) {
|
|
j.m_islandFlag = false;
|
|
}
|
|
// Build and simulate all awake islands.
|
|
var stack = this.m_stack;
|
|
for (var seed = world.m_bodyList; seed; seed = seed.m_next) {
|
|
if (seed.m_islandFlag) {
|
|
continue;
|
|
}
|
|
if (seed.isAwake() == false || seed.isActive() == false) {
|
|
continue;
|
|
}
|
|
// The seed can be dynamic or kinematic.
|
|
if (seed.isStatic()) {
|
|
continue;
|
|
}
|
|
// Reset island and stack.
|
|
this.clear();
|
|
stack.push(seed);
|
|
seed.m_islandFlag = true;
|
|
// Perform a depth first search (DFS) on the constraint graph.
|
|
while (stack.length > 0) {
|
|
// Grab the next body off the stack and add it to the island.
|
|
var b = stack.pop();
|
|
this.addBody(b);
|
|
// Make sure the body is awake.
|
|
b.setAwake(true);
|
|
// To keep islands as small as possible, we don't
|
|
// propagate islands across static bodies.
|
|
if (b.isStatic()) {
|
|
continue;
|
|
}
|
|
// Search all contacts connected to this body.
|
|
for (var ce = b.m_contactList; ce; ce = ce.next) {
|
|
var contact = ce.contact;
|
|
// Has this contact already been added to an island?
|
|
if (contact.m_islandFlag) {
|
|
continue;
|
|
}
|
|
// Is this contact solid and touching?
|
|
if (contact.isEnabled() == false || contact.isTouching() == false) {
|
|
continue;
|
|
}
|
|
// Skip sensors.
|
|
var sensorA = contact.m_fixtureA.m_isSensor;
|
|
var sensorB = contact.m_fixtureB.m_isSensor;
|
|
if (sensorA || sensorB) {
|
|
continue;
|
|
}
|
|
this.addContact(contact);
|
|
contact.m_islandFlag = true;
|
|
var other = ce.other;
|
|
// Was the other body already added to this island?
|
|
if (other.m_islandFlag) {
|
|
continue;
|
|
}
|
|
// _ASSERT && common.assert(stack.length < world.m_bodyCount);
|
|
stack.push(other);
|
|
other.m_islandFlag = true;
|
|
}
|
|
// Search all joints connect to this body.
|
|
for (var je = b.m_jointList; je; je = je.next) {
|
|
if (je.joint.m_islandFlag == true) {
|
|
continue;
|
|
}
|
|
var other = je.other;
|
|
// Don't simulate joints connected to inactive bodies.
|
|
if (other.isActive() == false) {
|
|
continue;
|
|
}
|
|
this.addJoint(je.joint);
|
|
je.joint.m_islandFlag = true;
|
|
if (other.m_islandFlag) {
|
|
continue;
|
|
}
|
|
// _ASSERT && common.assert(stack.length < world.m_bodyCount);
|
|
stack.push(other);
|
|
other.m_islandFlag = true;
|
|
}
|
|
}
|
|
this.solveIsland(step);
|
|
// Post solve cleanup.
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
// Allow static bodies to participate in other islands.
|
|
// TODO: are they added at all?
|
|
var b = this.m_bodies[i];
|
|
if (b.isStatic()) {
|
|
b.m_islandFlag = false;
|
|
}
|
|
}
|
|
}
|
|
};
|
|
Solver.prototype.solveIsland = function (step) {
|
|
// B2: Island Solve
|
|
var world = this.m_world;
|
|
var gravity = world.m_gravity;
|
|
var allowSleep = world.m_allowSleep;
|
|
var h = step.dt;
|
|
// Integrate velocities and apply damping. Initialize the body state.
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
var c = Vec2.clone(body.m_sweep.c);
|
|
var a = body.m_sweep.a;
|
|
var v = Vec2.clone(body.m_linearVelocity);
|
|
var w = body.m_angularVelocity;
|
|
// Store positions for continuous collision.
|
|
body.m_sweep.c0.set(body.m_sweep.c);
|
|
body.m_sweep.a0 = body.m_sweep.a;
|
|
if (body.isDynamic()) {
|
|
// Integrate velocities.
|
|
v.addMul(h * body.m_gravityScale, gravity);
|
|
v.addMul(h * body.m_invMass, body.m_force);
|
|
w += h * body.m_invI * body.m_torque;
|
|
/**
|
|
* <pre>
|
|
* Apply damping.
|
|
* ODE: dv/dt + c * v = 0
|
|
* Solution: v(t) = v0 * exp(-c * t)
|
|
* Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
|
|
* v2 = exp(-c * dt) * v1
|
|
* Pade approximation:
|
|
* v2 = v1 * 1 / (1 + c * dt)
|
|
* </pre>
|
|
*/
|
|
v.mul(1.0 / (1.0 + h * body.m_linearDamping));
|
|
w *= 1.0 / (1.0 + h * body.m_angularDamping);
|
|
}
|
|
body.c_position.c = c;
|
|
body.c_position.a = a;
|
|
body.c_velocity.v = v;
|
|
body.c_velocity.w = w;
|
|
}
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.initConstraint(step);
|
|
}
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.initVelocityConstraint(step);
|
|
}
|
|
if (step.warmStarting) {
|
|
// Warm start.
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.warmStartConstraint(step);
|
|
}
|
|
}
|
|
for (var i = 0; i < this.m_joints.length; ++i) {
|
|
var joint = this.m_joints[i];
|
|
joint.initVelocityConstraints(step);
|
|
}
|
|
// Solve velocity constraints
|
|
for (var i = 0; i < step.velocityIterations; ++i) {
|
|
for (var j = 0; j < this.m_joints.length; ++j) {
|
|
var joint = this.m_joints[j];
|
|
joint.solveVelocityConstraints(step);
|
|
}
|
|
for (var j = 0; j < this.m_contacts.length; ++j) {
|
|
var contact = this.m_contacts[j];
|
|
contact.solveVelocityConstraint(step);
|
|
}
|
|
}
|
|
// Store impulses for warm starting
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.storeConstraintImpulses(step);
|
|
}
|
|
// Integrate positions
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
var c = Vec2.clone(body.c_position.c);
|
|
var a = body.c_position.a;
|
|
var v = Vec2.clone(body.c_velocity.v);
|
|
var w = body.c_velocity.w;
|
|
// Check for large velocities
|
|
var translation = Vec2.mul(h, v);
|
|
if (Vec2.lengthSquared(translation) > Settings.maxTranslationSquared) {
|
|
var ratio = Settings.maxTranslation / translation.length();
|
|
v.mul(ratio);
|
|
}
|
|
var rotation = h * w;
|
|
if (rotation * rotation > Settings.maxRotationSquared) {
|
|
var ratio = Settings.maxRotation / math.abs(rotation);
|
|
w *= ratio;
|
|
}
|
|
// Integrate
|
|
c.addMul(h, v);
|
|
a += h * w;
|
|
body.c_position.c.set(c);
|
|
body.c_position.a = a;
|
|
body.c_velocity.v.set(v);
|
|
body.c_velocity.w = w;
|
|
}
|
|
// Solve position constraints
|
|
var positionSolved = false;
|
|
for (var i = 0; i < step.positionIterations; ++i) {
|
|
var minSeparation = 0.0;
|
|
for (var j = 0; j < this.m_contacts.length; ++j) {
|
|
var contact = this.m_contacts[j];
|
|
var separation = contact.solvePositionConstraint(step);
|
|
minSeparation = math.min(minSeparation, separation);
|
|
}
|
|
// We can't expect minSpeparation >= -Settings.linearSlop because we don't
|
|
// push the separation above -Settings.linearSlop.
|
|
var contactsOkay = minSeparation >= -3.0 * Settings.linearSlop;
|
|
var jointsOkay = true;
|
|
for (var j = 0; j < this.m_joints.length; ++j) {
|
|
var joint = this.m_joints[j];
|
|
var jointOkay = joint.solvePositionConstraints(step);
|
|
jointsOkay = jointsOkay && jointOkay;
|
|
}
|
|
if (contactsOkay && jointsOkay) {
|
|
// Exit early if the position errors are small.
|
|
positionSolved = true;
|
|
break;
|
|
}
|
|
}
|
|
// Copy state buffers back to the bodies
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
body.m_sweep.c.set(body.c_position.c);
|
|
body.m_sweep.a = body.c_position.a;
|
|
body.m_linearVelocity.set(body.c_velocity.v);
|
|
body.m_angularVelocity = body.c_velocity.w;
|
|
body.synchronizeTransform();
|
|
}
|
|
this.postSolveIsland();
|
|
if (allowSleep) {
|
|
var minSleepTime = Infinity;
|
|
var linTolSqr = Settings.linearSleepToleranceSqr;
|
|
var angTolSqr = Settings.angularSleepToleranceSqr;
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
if (body.isStatic()) {
|
|
continue;
|
|
}
|
|
if ((body.m_autoSleepFlag == false)
|
|
|| (body.m_angularVelocity * body.m_angularVelocity > angTolSqr)
|
|
|| (Vec2.lengthSquared(body.m_linearVelocity) > linTolSqr)) {
|
|
body.m_sleepTime = 0.0;
|
|
minSleepTime = 0.0;
|
|
}
|
|
else {
|
|
body.m_sleepTime += h;
|
|
minSleepTime = math.min(minSleepTime, body.m_sleepTime);
|
|
}
|
|
}
|
|
if (minSleepTime >= Settings.timeToSleep && positionSolved) {
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
body.setAwake(false);
|
|
}
|
|
}
|
|
}
|
|
};
|
|
Solver.prototype.printBodies = function (tag) {
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var b = this.m_bodies[i];
|
|
common.debug(tag, b.c_position.a, b.c_position.c.x, b.c_position.c.y, b.c_velocity.w, b.c_velocity.v.x, b.c_velocity.v.y);
|
|
}
|
|
};
|
|
/**
|
|
* Find TOI contacts and solve them.
|
|
*/
|
|
Solver.prototype.solveWorldTOI = function (step) {
|
|
var world = this.m_world;
|
|
if (world.m_stepComplete) {
|
|
for (var b = world.m_bodyList; b; b = b.m_next) {
|
|
b.m_islandFlag = false;
|
|
b.m_sweep.alpha0 = 0.0;
|
|
}
|
|
for (var c = world.m_contactList; c; c = c.m_next) {
|
|
// Invalidate TOI
|
|
c.m_toiFlag = false;
|
|
c.m_islandFlag = false;
|
|
c.m_toiCount = 0;
|
|
c.m_toi = 1.0;
|
|
}
|
|
}
|
|
// Find TOI events and solve them.
|
|
while (true) {
|
|
// Find the first TOI.
|
|
var minContact = null; // Contact
|
|
var minAlpha = 1.0;
|
|
for (var c = world.m_contactList; c; c = c.m_next) {
|
|
// Is this contact disabled?
|
|
if (c.isEnabled() == false) {
|
|
continue;
|
|
}
|
|
// Prevent excessive sub-stepping.
|
|
if (c.m_toiCount > Settings.maxSubSteps) {
|
|
continue;
|
|
}
|
|
var alpha = 1.0;
|
|
if (c.m_toiFlag) {
|
|
// This contact has a valid cached TOI.
|
|
alpha = c.m_toi;
|
|
}
|
|
else {
|
|
var fA_1 = c.getFixtureA();
|
|
var fB_1 = c.getFixtureB();
|
|
// Is there a sensor?
|
|
if (fA_1.isSensor() || fB_1.isSensor()) {
|
|
continue;
|
|
}
|
|
var bA_1 = fA_1.getBody();
|
|
var bB_1 = fB_1.getBody();
|
|
var activeA = bA_1.isAwake() && !bA_1.isStatic();
|
|
var activeB = bB_1.isAwake() && !bB_1.isStatic();
|
|
// Is at least one body active (awake and dynamic or kinematic)?
|
|
if (activeA == false && activeB == false) {
|
|
continue;
|
|
}
|
|
var collideA = bA_1.isBullet() || !bA_1.isDynamic();
|
|
var collideB = bB_1.isBullet() || !bB_1.isDynamic();
|
|
// Are these two non-bullet dynamic bodies?
|
|
if (collideA == false && collideB == false) {
|
|
continue;
|
|
}
|
|
// Compute the TOI for this contact.
|
|
// Put the sweeps onto the same time interval.
|
|
var alpha0 = bA_1.m_sweep.alpha0;
|
|
if (bA_1.m_sweep.alpha0 < bB_1.m_sweep.alpha0) {
|
|
alpha0 = bB_1.m_sweep.alpha0;
|
|
bA_1.m_sweep.advance(alpha0);
|
|
}
|
|
else if (bB_1.m_sweep.alpha0 < bA_1.m_sweep.alpha0) {
|
|
alpha0 = bA_1.m_sweep.alpha0;
|
|
bB_1.m_sweep.advance(alpha0);
|
|
}
|
|
var indexA = c.getChildIndexA();
|
|
var indexB = c.getChildIndexB();
|
|
bA_1.m_sweep;
|
|
bB_1.m_sweep;
|
|
// Compute the time of impact in interval [0, minTOI]
|
|
var input = new TOIInput(); // TODO: reuse
|
|
input.proxyA.set(fA_1.getShape(), indexA);
|
|
input.proxyB.set(fB_1.getShape(), indexB);
|
|
input.sweepA.set(bA_1.m_sweep);
|
|
input.sweepB.set(bB_1.m_sweep);
|
|
input.tMax = 1.0;
|
|
var output = new TOIOutput(); // TODO: reuse
|
|
TimeOfImpact(output, input);
|
|
// Beta is the fraction of the remaining portion of the [time?].
|
|
var beta = output.t;
|
|
if (output.state == TOIOutputState.e_touching) {
|
|
alpha = math.min(alpha0 + (1.0 - alpha0) * beta, 1.0);
|
|
}
|
|
else {
|
|
alpha = 1.0;
|
|
}
|
|
c.m_toi = alpha;
|
|
c.m_toiFlag = true;
|
|
}
|
|
if (alpha < minAlpha) {
|
|
// This is the minimum TOI found so far.
|
|
minContact = c;
|
|
minAlpha = alpha;
|
|
}
|
|
}
|
|
if (minContact == null || 1.0 - 10.0 * math.EPSILON < minAlpha) {
|
|
// No more TOI events. Done!
|
|
world.m_stepComplete = true;
|
|
break;
|
|
}
|
|
// Advance the bodies to the TOI.
|
|
var fA = minContact.getFixtureA();
|
|
var fB = minContact.getFixtureB();
|
|
var bA = fA.getBody();
|
|
var bB = fB.getBody();
|
|
var backup1 = bA.m_sweep.clone();
|
|
var backup2 = bB.m_sweep.clone();
|
|
bA.advance(minAlpha);
|
|
bB.advance(minAlpha);
|
|
// The TOI contact likely has some new contact points.
|
|
minContact.update(world);
|
|
minContact.m_toiFlag = false;
|
|
++minContact.m_toiCount;
|
|
// Is the contact solid?
|
|
if (minContact.isEnabled() == false || minContact.isTouching() == false) {
|
|
// Restore the sweeps.
|
|
minContact.setEnabled(false);
|
|
bA.m_sweep.set(backup1);
|
|
bB.m_sweep.set(backup2);
|
|
bA.synchronizeTransform();
|
|
bB.synchronizeTransform();
|
|
continue;
|
|
}
|
|
bA.setAwake(true);
|
|
bB.setAwake(true);
|
|
// Build the island
|
|
this.clear();
|
|
this.addBody(bA);
|
|
this.addBody(bB);
|
|
this.addContact(minContact);
|
|
bA.m_islandFlag = true;
|
|
bB.m_islandFlag = true;
|
|
minContact.m_islandFlag = true;
|
|
// Get contacts on bodyA and bodyB.
|
|
var bodies = [bA, bB];
|
|
for (var i = 0; i < bodies.length; ++i) {
|
|
var body = bodies[i];
|
|
if (body.isDynamic()) {
|
|
for (var ce = body.m_contactList; ce; ce = ce.next) {
|
|
// if (this.m_bodyCount == this.m_bodyCapacity) { break; }
|
|
// if (this.m_contactCount == this.m_contactCapacity) { break; }
|
|
var contact = ce.contact;
|
|
// Has this contact already been added to the island?
|
|
if (contact.m_islandFlag) {
|
|
continue;
|
|
}
|
|
// Only add if either is static, kinematic or bullet.
|
|
var other = ce.other;
|
|
if (other.isDynamic() && !body.isBullet() && !other.isBullet()) {
|
|
continue;
|
|
}
|
|
// Skip sensors.
|
|
var sensorA = contact.m_fixtureA.m_isSensor;
|
|
var sensorB = contact.m_fixtureB.m_isSensor;
|
|
if (sensorA || sensorB) {
|
|
continue;
|
|
}
|
|
// Tentatively advance the body to the TOI.
|
|
var backup = other.m_sweep.clone();
|
|
if (other.m_islandFlag == false) {
|
|
other.advance(minAlpha);
|
|
}
|
|
// Update the contact points
|
|
contact.update(world);
|
|
// Was the contact disabled by the user?
|
|
// Are there contact points?
|
|
if (contact.isEnabled() == false || contact.isTouching() == false) {
|
|
other.m_sweep.set(backup);
|
|
other.synchronizeTransform();
|
|
continue;
|
|
}
|
|
// Add the contact to the island
|
|
contact.m_islandFlag = true;
|
|
this.addContact(contact);
|
|
// Has the other body already been added to the island?
|
|
if (other.m_islandFlag) {
|
|
continue;
|
|
}
|
|
// Add the other body to the island.
|
|
other.m_islandFlag = true;
|
|
if (!other.isStatic()) {
|
|
other.setAwake(true);
|
|
}
|
|
this.addBody(other);
|
|
}
|
|
}
|
|
}
|
|
s_subStep.reset((1.0 - minAlpha) * step.dt);
|
|
s_subStep.dtRatio = 1.0;
|
|
s_subStep.positionIterations = 20;
|
|
s_subStep.velocityIterations = step.velocityIterations;
|
|
s_subStep.warmStarting = false;
|
|
this.solveIslandTOI(s_subStep, bA, bB);
|
|
// Reset island flags and synchronize broad-phase proxies.
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
body.m_islandFlag = false;
|
|
if (!body.isDynamic()) {
|
|
continue;
|
|
}
|
|
body.synchronizeFixtures();
|
|
// Invalidate all contact TOIs on this displaced body.
|
|
for (var ce = body.m_contactList; ce; ce = ce.next) {
|
|
ce.contact.m_toiFlag = false;
|
|
ce.contact.m_islandFlag = false;
|
|
}
|
|
}
|
|
// Commit fixture proxy movements to the broad-phase so that new contacts
|
|
// are created.
|
|
// Also, some contacts can be destroyed.
|
|
world.findNewContacts();
|
|
if (world.m_subStepping) {
|
|
world.m_stepComplete = false;
|
|
break;
|
|
}
|
|
}
|
|
var b, c;
|
|
};
|
|
Solver.prototype.solveIslandTOI = function (subStep, toiA, toiB) {
|
|
this.m_world;
|
|
// Initialize the body state.
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
body.c_position.c.set(body.m_sweep.c);
|
|
body.c_position.a = body.m_sweep.a;
|
|
body.c_velocity.v.set(body.m_linearVelocity);
|
|
body.c_velocity.w = body.m_angularVelocity;
|
|
}
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.initConstraint(subStep);
|
|
}
|
|
// Solve position constraints.
|
|
for (var i = 0; i < subStep.positionIterations; ++i) {
|
|
var minSeparation = 0.0;
|
|
for (var j = 0; j < this.m_contacts.length; ++j) {
|
|
var contact = this.m_contacts[j];
|
|
var separation = contact.solvePositionConstraintTOI(subStep, toiA, toiB);
|
|
minSeparation = math.min(minSeparation, separation);
|
|
}
|
|
// We can't expect minSpeparation >= -Settings.linearSlop because we don't
|
|
// push the separation above -Settings.linearSlop.
|
|
var contactsOkay = minSeparation >= -1.5 * Settings.linearSlop;
|
|
if (contactsOkay) {
|
|
break;
|
|
}
|
|
}
|
|
var i, c;
|
|
// Leap of faith to new safe state.
|
|
toiA.m_sweep.c0.set(toiA.c_position.c);
|
|
toiA.m_sweep.a0 = toiA.c_position.a;
|
|
toiB.m_sweep.c0.set(toiB.c_position.c);
|
|
toiB.m_sweep.a0 = toiB.c_position.a;
|
|
// No warm starting is needed for TOI events because warm
|
|
// starting impulses were applied in the discrete solver.
|
|
for (var i = 0; i < this.m_contacts.length; ++i) {
|
|
var contact = this.m_contacts[i];
|
|
contact.initVelocityConstraint(subStep);
|
|
}
|
|
// Solve velocity constraints.
|
|
for (var i = 0; i < subStep.velocityIterations; ++i) {
|
|
for (var j = 0; j < this.m_contacts.length; ++j) {
|
|
var contact = this.m_contacts[j];
|
|
contact.solveVelocityConstraint(subStep);
|
|
}
|
|
}
|
|
// Don't store the TOI contact forces for warm starting
|
|
// because they can be quite large.
|
|
var h = subStep.dt;
|
|
// Integrate positions
|
|
for (var i = 0; i < this.m_bodies.length; ++i) {
|
|
var body = this.m_bodies[i];
|
|
var c = Vec2.clone(body.c_position.c);
|
|
var a = body.c_position.a;
|
|
var v = Vec2.clone(body.c_velocity.v);
|
|
var w = body.c_velocity.w;
|
|
// Check for large velocities
|
|
var translation = Vec2.mul(h, v);
|
|
if (Vec2.dot(translation, translation) > Settings.maxTranslationSquared) {
|
|
var ratio = Settings.maxTranslation / translation.length();
|
|
v.mul(ratio);
|
|
}
|
|
var rotation = h * w;
|
|
if (rotation * rotation > Settings.maxRotationSquared) {
|
|
var ratio = Settings.maxRotation / math.abs(rotation);
|
|
w *= ratio;
|
|
}
|
|
// Integrate
|
|
c.addMul(h, v);
|
|
a += h * w;
|
|
body.c_position.c = c;
|
|
body.c_position.a = a;
|
|
body.c_velocity.v = v;
|
|
body.c_velocity.w = w;
|
|
// Sync bodies
|
|
body.m_sweep.c = c;
|
|
body.m_sweep.a = a;
|
|
body.m_linearVelocity = v;
|
|
body.m_angularVelocity = w;
|
|
body.synchronizeTransform();
|
|
}
|
|
this.postSolveIsland();
|
|
};
|
|
Solver.prototype.postSolveIsland = function () {
|
|
for (var c = 0; c < this.m_contacts.length; ++c) {
|
|
var contact = this.m_contacts[c];
|
|
this.m_world.postSolve(contact, contact.m_impulse);
|
|
}
|
|
};
|
|
return Solver;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var WorldDefDefault = {
|
|
gravity: Vec2.zero(),
|
|
allowSleep: true,
|
|
warmStarting: true,
|
|
continuousPhysics: true,
|
|
subStepping: false,
|
|
blockSolve: true,
|
|
velocityIterations: 8,
|
|
positionIterations: 3
|
|
};
|
|
var World = /** @class */ (function () {
|
|
/**
|
|
* @param def World definition or gravity vector.
|
|
*/
|
|
function World(def) {
|
|
var _this = this;
|
|
/** @internal */
|
|
this.s_step = new TimeStep(); // reuse
|
|
/**
|
|
* @internal
|
|
* Callback for broad-phase.
|
|
*/
|
|
this.createContact = function (proxyA, proxyB) {
|
|
var fixtureA = proxyA.fixture;
|
|
var fixtureB = proxyB.fixture;
|
|
var indexA = proxyA.childIndex;
|
|
var indexB = proxyB.childIndex;
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
// Are the fixtures on the same body?
|
|
if (bodyA == bodyB) {
|
|
return;
|
|
}
|
|
// TODO_ERIN use a hash table to remove a potential bottleneck when both
|
|
// bodies have a lot of contacts.
|
|
// Does a contact already exist?
|
|
var edge = bodyB.getContactList(); // ContactEdge
|
|
while (edge) {
|
|
if (edge.other == bodyA) {
|
|
var fA = edge.contact.getFixtureA();
|
|
var fB = edge.contact.getFixtureB();
|
|
var iA = edge.contact.getChildIndexA();
|
|
var iB = edge.contact.getChildIndexB();
|
|
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) {
|
|
// A contact already exists.
|
|
return;
|
|
}
|
|
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) {
|
|
// A contact already exists.
|
|
return;
|
|
}
|
|
}
|
|
edge = edge.next;
|
|
}
|
|
if (bodyB.shouldCollide(bodyA) == false) {
|
|
return;
|
|
}
|
|
if (fixtureB.shouldCollide(fixtureA) == false) {
|
|
return;
|
|
}
|
|
// Call the factory.
|
|
var contact = Contact.create(fixtureA, indexA, fixtureB, indexB);
|
|
if (contact == null) {
|
|
return;
|
|
}
|
|
// Insert into the world.
|
|
contact.m_prev = null;
|
|
if (_this.m_contactList != null) {
|
|
contact.m_next = _this.m_contactList;
|
|
_this.m_contactList.m_prev = contact;
|
|
}
|
|
_this.m_contactList = contact;
|
|
++_this.m_contactCount;
|
|
};
|
|
if (!(this instanceof World)) {
|
|
return new World(def);
|
|
}
|
|
if (def && Vec2.isValid(def)) {
|
|
def = { gravity: def };
|
|
}
|
|
def = options(def, WorldDefDefault);
|
|
this.m_solver = new Solver(this);
|
|
this.m_broadPhase = new BroadPhase();
|
|
this.m_contactList = null;
|
|
this.m_contactCount = 0;
|
|
this.m_bodyList = null;
|
|
this.m_bodyCount = 0;
|
|
this.m_jointList = null;
|
|
this.m_jointCount = 0;
|
|
this.m_stepComplete = true;
|
|
this.m_allowSleep = def.allowSleep;
|
|
this.m_gravity = Vec2.clone(def.gravity);
|
|
this.m_clearForces = true;
|
|
this.m_newFixture = false;
|
|
this.m_locked = false;
|
|
// These are for debugging the solver.
|
|
this.m_warmStarting = def.warmStarting;
|
|
this.m_continuousPhysics = def.continuousPhysics;
|
|
this.m_subStepping = def.subStepping;
|
|
this.m_blockSolve = def.blockSolve;
|
|
this.m_velocityIterations = def.velocityIterations;
|
|
this.m_positionIterations = def.positionIterations;
|
|
this.m_t = 0;
|
|
}
|
|
/** @internal */
|
|
World.prototype._serialize = function () {
|
|
var bodies = [];
|
|
var joints = [];
|
|
for (var b = this.getBodyList(); b; b = b.getNext()) {
|
|
bodies.push(b);
|
|
}
|
|
for (var j = this.getJointList(); j; j = j.getNext()) {
|
|
// @ts-ignore
|
|
if (typeof j._serialize === 'function') {
|
|
joints.push(j);
|
|
}
|
|
}
|
|
return {
|
|
gravity: this.m_gravity,
|
|
bodies: bodies,
|
|
joints: joints,
|
|
};
|
|
};
|
|
/** @internal */
|
|
World._deserialize = function (data, context, restore) {
|
|
if (!data) {
|
|
return new World();
|
|
}
|
|
var world = new World(data.gravity);
|
|
if (data.bodies) {
|
|
for (var i = data.bodies.length - 1; i >= 0; i -= 1) {
|
|
world._addBody(restore(Body, data.bodies[i], world));
|
|
}
|
|
}
|
|
if (data.joints) {
|
|
for (var i = data.joints.length - 1; i >= 0; i--) {
|
|
world.createJoint(restore(Joint, data.joints[i], world));
|
|
}
|
|
}
|
|
return world;
|
|
};
|
|
/**
|
|
* Get the world body list. With the returned body, use Body.getNext to get the
|
|
* next body in the world list. A null body indicates the end of the list.
|
|
*
|
|
* @return the head of the world body list.
|
|
*/
|
|
World.prototype.getBodyList = function () {
|
|
return this.m_bodyList;
|
|
};
|
|
/**
|
|
* Get the world joint list. With the returned joint, use Joint.getNext to get
|
|
* the next joint in the world list. A null joint indicates the end of the list.
|
|
*
|
|
* @return the head of the world joint list.
|
|
*/
|
|
World.prototype.getJointList = function () {
|
|
return this.m_jointList;
|
|
};
|
|
/**
|
|
* Get the world contact list. With the returned contact, use Contact.getNext to
|
|
* get the next contact in the world list. A null contact indicates the end of
|
|
* the list.
|
|
*
|
|
* Warning: contacts are created and destroyed in the middle of a time step.
|
|
* Use ContactListener to avoid missing contacts.
|
|
*
|
|
* @return the head of the world contact list.
|
|
*/
|
|
World.prototype.getContactList = function () {
|
|
return this.m_contactList;
|
|
};
|
|
World.prototype.getBodyCount = function () {
|
|
return this.m_bodyCount;
|
|
};
|
|
World.prototype.getJointCount = function () {
|
|
return this.m_jointCount;
|
|
};
|
|
/**
|
|
* Get the number of contacts (each may have 0 or more contact points).
|
|
*/
|
|
World.prototype.getContactCount = function () {
|
|
return this.m_contactCount;
|
|
};
|
|
/**
|
|
* Change the global gravity vector.
|
|
*/
|
|
World.prototype.setGravity = function (gravity) {
|
|
this.m_gravity = gravity;
|
|
};
|
|
/**
|
|
* Get the global gravity vector.
|
|
*/
|
|
World.prototype.getGravity = function () {
|
|
return this.m_gravity;
|
|
};
|
|
/**
|
|
* Is the world locked (in the middle of a time step).
|
|
*/
|
|
World.prototype.isLocked = function () {
|
|
return this.m_locked;
|
|
};
|
|
/**
|
|
* Enable/disable sleep.
|
|
*/
|
|
World.prototype.setAllowSleeping = function (flag) {
|
|
if (flag == this.m_allowSleep) {
|
|
return;
|
|
}
|
|
this.m_allowSleep = flag;
|
|
if (this.m_allowSleep == false) {
|
|
for (var b = this.m_bodyList; b; b = b.m_next) {
|
|
b.setAwake(true);
|
|
}
|
|
}
|
|
};
|
|
World.prototype.getAllowSleeping = function () {
|
|
return this.m_allowSleep;
|
|
};
|
|
/**
|
|
* Enable/disable warm starting. For testing.
|
|
*/
|
|
World.prototype.setWarmStarting = function (flag) {
|
|
this.m_warmStarting = flag;
|
|
};
|
|
World.prototype.getWarmStarting = function () {
|
|
return this.m_warmStarting;
|
|
};
|
|
/**
|
|
* Enable/disable continuous physics. For testing.
|
|
*/
|
|
World.prototype.setContinuousPhysics = function (flag) {
|
|
this.m_continuousPhysics = flag;
|
|
};
|
|
World.prototype.getContinuousPhysics = function () {
|
|
return this.m_continuousPhysics;
|
|
};
|
|
/**
|
|
* Enable/disable single stepped continuous physics. For testing.
|
|
*/
|
|
World.prototype.setSubStepping = function (flag) {
|
|
this.m_subStepping = flag;
|
|
};
|
|
World.prototype.getSubStepping = function () {
|
|
return this.m_subStepping;
|
|
};
|
|
/**
|
|
* Set flag to control automatic clearing of forces after each time step.
|
|
*/
|
|
World.prototype.setAutoClearForces = function (flag) {
|
|
this.m_clearForces = flag;
|
|
};
|
|
/**
|
|
* Get the flag that controls automatic clearing of forces after each time step.
|
|
*/
|
|
World.prototype.getAutoClearForces = function () {
|
|
return this.m_clearForces;
|
|
};
|
|
/**
|
|
* Manually clear the force buffer on all bodies. By default, forces are cleared
|
|
* automatically after each call to step. The default behavior is modified by
|
|
* calling setAutoClearForces. The purpose of this function is to support
|
|
* sub-stepping. Sub-stepping is often used to maintain a fixed sized time step
|
|
* under a variable frame-rate. When you perform sub-stepping you will disable
|
|
* auto clearing of forces and instead call clearForces after all sub-steps are
|
|
* complete in one pass of your game loop.
|
|
*
|
|
* @see setAutoClearForces
|
|
*/
|
|
World.prototype.clearForces = function () {
|
|
for (var body = this.m_bodyList; body; body = body.getNext()) {
|
|
body.m_force.setZero();
|
|
body.m_torque = 0.0;
|
|
}
|
|
};
|
|
/**
|
|
* Query the world for all fixtures that potentially overlap the provided AABB.
|
|
*
|
|
* @param aabb The query box.
|
|
* @param callback Called for each fixture found in the query AABB. It may return `false` to terminate the query.
|
|
*/
|
|
World.prototype.queryAABB = function (aabb, callback) {
|
|
var broadPhase = this.m_broadPhase;
|
|
this.m_broadPhase.query(aabb, function (proxyId) {
|
|
var proxy = broadPhase.getUserData(proxyId);
|
|
return callback(proxy.fixture);
|
|
});
|
|
};
|
|
/**
|
|
*
|
|
* Ray-cast the world for all fixtures in the path of the ray. Your callback
|
|
* controls whether you get the closest point, any point, or n-points. The
|
|
* ray-cast ignores shapes that contain the starting point.
|
|
*
|
|
* @param point1 The ray starting point
|
|
* @param point2 The ray ending point
|
|
* @param callback A user implemented callback function.
|
|
*/
|
|
World.prototype.rayCast = function (point1, point2, callback) {
|
|
var broadPhase = this.m_broadPhase;
|
|
this.m_broadPhase.rayCast({
|
|
maxFraction: 1.0,
|
|
p1: point1,
|
|
p2: point2
|
|
}, function (input, proxyId) {
|
|
var proxy = broadPhase.getUserData(proxyId);
|
|
var fixture = proxy.fixture;
|
|
var index = proxy.childIndex;
|
|
var output = {}; // TODO GC
|
|
var hit = fixture.rayCast(output, input, index);
|
|
if (hit) {
|
|
var fraction = output.fraction;
|
|
var point = Vec2.add(Vec2.mul((1.0 - fraction), input.p1), Vec2.mul(fraction, input.p2));
|
|
return callback(fixture, point, output.normal, fraction);
|
|
}
|
|
return input.maxFraction;
|
|
});
|
|
};
|
|
/**
|
|
* Get the number of broad-phase proxies.
|
|
*/
|
|
World.prototype.getProxyCount = function () {
|
|
return this.m_broadPhase.getProxyCount();
|
|
};
|
|
/**
|
|
* Get the height of broad-phase dynamic tree.
|
|
*/
|
|
World.prototype.getTreeHeight = function () {
|
|
return this.m_broadPhase.getTreeHeight();
|
|
};
|
|
/**
|
|
* Get the balance of broad-phase dynamic tree.
|
|
*/
|
|
World.prototype.getTreeBalance = function () {
|
|
return this.m_broadPhase.getTreeBalance();
|
|
};
|
|
/**
|
|
* Get the quality metric of broad-phase dynamic tree. The smaller the better.
|
|
* The minimum is 1.
|
|
*/
|
|
World.prototype.getTreeQuality = function () {
|
|
return this.m_broadPhase.getTreeQuality();
|
|
};
|
|
/**
|
|
* Shift the world origin. Useful for large worlds. The body shift formula is:
|
|
* position -= newOrigin
|
|
*
|
|
* @param newOrigin The new origin with respect to the old origin
|
|
*/
|
|
World.prototype.shiftOrigin = function (newOrigin) {
|
|
if (this.m_locked) {
|
|
return;
|
|
}
|
|
for (var b = this.m_bodyList; b; b = b.m_next) {
|
|
b.m_xf.p.sub(newOrigin);
|
|
b.m_sweep.c0.sub(newOrigin);
|
|
b.m_sweep.c.sub(newOrigin);
|
|
}
|
|
for (var j = this.m_jointList; j; j = j.m_next) {
|
|
j.shiftOrigin(newOrigin);
|
|
}
|
|
this.m_broadPhase.shiftOrigin(newOrigin);
|
|
};
|
|
/**
|
|
* @internal Used for deserialize.
|
|
*/
|
|
World.prototype._addBody = function (body) {
|
|
if (this.isLocked()) {
|
|
return;
|
|
}
|
|
// Add to world doubly linked list.
|
|
body.m_prev = null;
|
|
body.m_next = this.m_bodyList;
|
|
if (this.m_bodyList) {
|
|
this.m_bodyList.m_prev = body;
|
|
}
|
|
this.m_bodyList = body;
|
|
++this.m_bodyCount;
|
|
};
|
|
World.prototype.createBody = function (arg1, arg2) {
|
|
if (this.isLocked()) {
|
|
return null;
|
|
}
|
|
var def = {};
|
|
if (!arg1) ;
|
|
else if (Vec2.isValid(arg1)) {
|
|
def = { position: arg1, angle: arg2 };
|
|
}
|
|
else if (typeof arg1 === 'object') {
|
|
def = arg1;
|
|
}
|
|
var body = new Body(this, def);
|
|
this._addBody(body);
|
|
return body;
|
|
};
|
|
World.prototype.createDynamicBody = function (arg1, arg2) {
|
|
var def = {};
|
|
if (!arg1) ;
|
|
else if (Vec2.isValid(arg1)) {
|
|
def = { position: arg1, angle: arg2 };
|
|
}
|
|
else if (typeof arg1 === 'object') {
|
|
def = arg1;
|
|
}
|
|
def.type = 'dynamic';
|
|
return this.createBody(def);
|
|
};
|
|
World.prototype.createKinematicBody = function (arg1, arg2) {
|
|
var def = {};
|
|
if (!arg1) ;
|
|
else if (Vec2.isValid(arg1)) {
|
|
def = { position: arg1, angle: arg2 };
|
|
}
|
|
else if (typeof arg1 === 'object') {
|
|
def = arg1;
|
|
}
|
|
def.type = 'kinematic';
|
|
return this.createBody(def);
|
|
};
|
|
/**
|
|
* Destroy a rigid body given a definition. No reference to the definition is
|
|
* retained.
|
|
*
|
|
* Warning: This automatically deletes all associated shapes and joints.
|
|
*
|
|
* Warning: This function is locked during callbacks.
|
|
*/
|
|
World.prototype.destroyBody = function (b) {
|
|
if (this.isLocked()) {
|
|
return;
|
|
}
|
|
if (b.m_destroyed) {
|
|
return false;
|
|
}
|
|
// Delete the attached joints.
|
|
var je = b.m_jointList;
|
|
while (je) {
|
|
var je0 = je;
|
|
je = je.next;
|
|
this.publish('remove-joint', je0.joint);
|
|
this.destroyJoint(je0.joint);
|
|
b.m_jointList = je;
|
|
}
|
|
b.m_jointList = null;
|
|
// Delete the attached contacts.
|
|
var ce = b.m_contactList;
|
|
while (ce) {
|
|
var ce0 = ce;
|
|
ce = ce.next;
|
|
this.destroyContact(ce0.contact);
|
|
b.m_contactList = ce;
|
|
}
|
|
b.m_contactList = null;
|
|
// Delete the attached fixtures. This destroys broad-phase proxies.
|
|
var f = b.m_fixtureList;
|
|
while (f) {
|
|
var f0 = f;
|
|
f = f.m_next;
|
|
this.publish('remove-fixture', f0);
|
|
f0.destroyProxies(this.m_broadPhase);
|
|
b.m_fixtureList = f;
|
|
}
|
|
b.m_fixtureList = null;
|
|
// Remove world body list.
|
|
if (b.m_prev) {
|
|
b.m_prev.m_next = b.m_next;
|
|
}
|
|
if (b.m_next) {
|
|
b.m_next.m_prev = b.m_prev;
|
|
}
|
|
if (b == this.m_bodyList) {
|
|
this.m_bodyList = b.m_next;
|
|
}
|
|
b.m_destroyed = true;
|
|
--this.m_bodyCount;
|
|
this.publish('remove-body', b);
|
|
return true;
|
|
};
|
|
/**
|
|
* Create a joint to constrain bodies together. No reference to the definition
|
|
* is retained. This may cause the connected bodies to cease colliding.
|
|
*
|
|
* Warning: This function is locked during callbacks.
|
|
*/
|
|
World.prototype.createJoint = function (joint) {
|
|
if (this.isLocked()) {
|
|
return null;
|
|
}
|
|
// Connect to the world list.
|
|
joint.m_prev = null;
|
|
joint.m_next = this.m_jointList;
|
|
if (this.m_jointList) {
|
|
this.m_jointList.m_prev = joint;
|
|
}
|
|
this.m_jointList = joint;
|
|
++this.m_jointCount;
|
|
// Connect to the bodies' doubly linked lists.
|
|
joint.m_edgeA.joint = joint;
|
|
joint.m_edgeA.other = joint.m_bodyB;
|
|
joint.m_edgeA.prev = null;
|
|
joint.m_edgeA.next = joint.m_bodyA.m_jointList;
|
|
if (joint.m_bodyA.m_jointList)
|
|
joint.m_bodyA.m_jointList.prev = joint.m_edgeA;
|
|
joint.m_bodyA.m_jointList = joint.m_edgeA;
|
|
joint.m_edgeB.joint = joint;
|
|
joint.m_edgeB.other = joint.m_bodyA;
|
|
joint.m_edgeB.prev = null;
|
|
joint.m_edgeB.next = joint.m_bodyB.m_jointList;
|
|
if (joint.m_bodyB.m_jointList)
|
|
joint.m_bodyB.m_jointList.prev = joint.m_edgeB;
|
|
joint.m_bodyB.m_jointList = joint.m_edgeB;
|
|
// If the joint prevents collisions, then flag any contacts for filtering.
|
|
if (joint.m_collideConnected == false) {
|
|
for (var edge = joint.m_bodyB.getContactList(); edge; edge = edge.next) {
|
|
if (edge.other == joint.m_bodyA) {
|
|
// Flag the contact for filtering at the next time step (where either
|
|
// body is awake).
|
|
edge.contact.flagForFiltering();
|
|
}
|
|
}
|
|
}
|
|
// Note: creating a joint doesn't wake the bodies.
|
|
return joint;
|
|
};
|
|
/**
|
|
* Destroy a joint. This may cause the connected bodies to begin colliding.
|
|
* Warning: This function is locked during callbacks.
|
|
*/
|
|
World.prototype.destroyJoint = function (joint) {
|
|
if (this.isLocked()) {
|
|
return;
|
|
}
|
|
// Remove from the doubly linked list.
|
|
if (joint.m_prev) {
|
|
joint.m_prev.m_next = joint.m_next;
|
|
}
|
|
if (joint.m_next) {
|
|
joint.m_next.m_prev = joint.m_prev;
|
|
}
|
|
if (joint == this.m_jointList) {
|
|
this.m_jointList = joint.m_next;
|
|
}
|
|
// Disconnect from bodies.
|
|
var bodyA = joint.m_bodyA;
|
|
var bodyB = joint.m_bodyB;
|
|
// Wake up connected bodies.
|
|
bodyA.setAwake(true);
|
|
bodyB.setAwake(true);
|
|
// Remove from body 1.
|
|
if (joint.m_edgeA.prev) {
|
|
joint.m_edgeA.prev.next = joint.m_edgeA.next;
|
|
}
|
|
if (joint.m_edgeA.next) {
|
|
joint.m_edgeA.next.prev = joint.m_edgeA.prev;
|
|
}
|
|
if (joint.m_edgeA == bodyA.m_jointList) {
|
|
bodyA.m_jointList = joint.m_edgeA.next;
|
|
}
|
|
joint.m_edgeA.prev = null;
|
|
joint.m_edgeA.next = null;
|
|
// Remove from body 2
|
|
if (joint.m_edgeB.prev) {
|
|
joint.m_edgeB.prev.next = joint.m_edgeB.next;
|
|
}
|
|
if (joint.m_edgeB.next) {
|
|
joint.m_edgeB.next.prev = joint.m_edgeB.prev;
|
|
}
|
|
if (joint.m_edgeB == bodyB.m_jointList) {
|
|
bodyB.m_jointList = joint.m_edgeB.next;
|
|
}
|
|
joint.m_edgeB.prev = null;
|
|
joint.m_edgeB.next = null;
|
|
--this.m_jointCount;
|
|
// If the joint prevents collisions, then flag any contacts for filtering.
|
|
if (joint.m_collideConnected == false) {
|
|
var edge = bodyB.getContactList();
|
|
while (edge) {
|
|
if (edge.other == bodyA) {
|
|
// Flag the contact for filtering at the next time step (where either
|
|
// body is awake).
|
|
edge.contact.flagForFiltering();
|
|
}
|
|
edge = edge.next;
|
|
}
|
|
}
|
|
this.publish('remove-joint', joint);
|
|
};
|
|
/**
|
|
* Take a time step. This performs collision detection, integration, and
|
|
* constraint solution.
|
|
*
|
|
* Broad-phase, narrow-phase, solve and solve time of impacts.
|
|
*
|
|
* @param timeStep Time step, this should not vary.
|
|
*/
|
|
World.prototype.step = function (timeStep, velocityIterations, positionIterations) {
|
|
this.publish('pre-step', timeStep);
|
|
if ((velocityIterations | 0) !== velocityIterations) {
|
|
// TODO: remove this in future
|
|
velocityIterations = 0;
|
|
}
|
|
velocityIterations = velocityIterations || this.m_velocityIterations;
|
|
positionIterations = positionIterations || this.m_positionIterations;
|
|
// If new fixtures were added, we need to find the new contacts.
|
|
if (this.m_newFixture) {
|
|
this.findNewContacts();
|
|
this.m_newFixture = false;
|
|
}
|
|
this.m_locked = true;
|
|
this.s_step.reset(timeStep);
|
|
this.s_step.velocityIterations = velocityIterations;
|
|
this.s_step.positionIterations = positionIterations;
|
|
this.s_step.warmStarting = this.m_warmStarting;
|
|
this.s_step.blockSolve = this.m_blockSolve;
|
|
// Update contacts. This is where some contacts are destroyed.
|
|
this.updateContacts();
|
|
// Integrate velocities, solve velocity constraints, and integrate positions.
|
|
if (this.m_stepComplete && timeStep > 0.0) {
|
|
this.m_solver.solveWorld(this.s_step);
|
|
// Synchronize fixtures, check for out of range bodies.
|
|
for (var b = this.m_bodyList; b; b = b.getNext()) {
|
|
// If a body was not in an island then it did not move.
|
|
if (b.m_islandFlag == false) {
|
|
continue;
|
|
}
|
|
if (b.isStatic()) {
|
|
continue;
|
|
}
|
|
// Update fixtures (for broad-phase).
|
|
b.synchronizeFixtures();
|
|
}
|
|
// Look for new contacts.
|
|
this.findNewContacts();
|
|
}
|
|
// Handle TOI events.
|
|
if (this.m_continuousPhysics && timeStep > 0.0) {
|
|
this.m_solver.solveWorldTOI(this.s_step);
|
|
}
|
|
if (this.m_clearForces) {
|
|
this.clearForces();
|
|
}
|
|
this.m_locked = false;
|
|
this.publish('post-step', timeStep);
|
|
};
|
|
/**
|
|
* @internal
|
|
* Call this method to find new contacts.
|
|
*/
|
|
World.prototype.findNewContacts = function () {
|
|
this.m_broadPhase.updatePairs(this.createContact);
|
|
};
|
|
/**
|
|
* @internal
|
|
* Removes old non-overlapping contacts, applies filters and updates contacts.
|
|
*/
|
|
World.prototype.updateContacts = function () {
|
|
// Update awake contacts.
|
|
var c;
|
|
var next_c = this.m_contactList;
|
|
while (c = next_c) {
|
|
next_c = c.getNext();
|
|
var fixtureA = c.getFixtureA();
|
|
var fixtureB = c.getFixtureB();
|
|
var indexA = c.getChildIndexA();
|
|
var indexB = c.getChildIndexB();
|
|
var bodyA = fixtureA.getBody();
|
|
var bodyB = fixtureB.getBody();
|
|
// Is this contact flagged for filtering?
|
|
if (c.m_filterFlag) {
|
|
if (bodyB.shouldCollide(bodyA) == false) {
|
|
this.destroyContact(c);
|
|
continue;
|
|
}
|
|
if (fixtureB.shouldCollide(fixtureA) == false) {
|
|
this.destroyContact(c);
|
|
continue;
|
|
}
|
|
// Clear the filtering flag.
|
|
c.m_filterFlag = false;
|
|
}
|
|
var activeA = bodyA.isAwake() && !bodyA.isStatic();
|
|
var activeB = bodyB.isAwake() && !bodyB.isStatic();
|
|
// At least one body must be awake and it must be dynamic or kinematic.
|
|
if (activeA == false && activeB == false) {
|
|
continue;
|
|
}
|
|
var proxyIdA = fixtureA.m_proxies[indexA].proxyId;
|
|
var proxyIdB = fixtureB.m_proxies[indexB].proxyId;
|
|
var overlap = this.m_broadPhase.testOverlap(proxyIdA, proxyIdB);
|
|
// Here we destroy contacts that cease to overlap in the broad-phase.
|
|
if (overlap == false) {
|
|
this.destroyContact(c);
|
|
continue;
|
|
}
|
|
// The contact persists.
|
|
c.update(this);
|
|
}
|
|
};
|
|
/**
|
|
* @internal
|
|
*/
|
|
World.prototype.destroyContact = function (contact) {
|
|
Contact.destroy(contact, this);
|
|
// Remove from the world.
|
|
if (contact.m_prev) {
|
|
contact.m_prev.m_next = contact.m_next;
|
|
}
|
|
if (contact.m_next) {
|
|
contact.m_next.m_prev = contact.m_prev;
|
|
}
|
|
if (contact == this.m_contactList) {
|
|
this.m_contactList = contact.m_next;
|
|
}
|
|
--this.m_contactCount;
|
|
};
|
|
/**
|
|
* Register an event listener.
|
|
*/
|
|
World.prototype.on = function (name, listener) {
|
|
if (typeof name !== 'string' || typeof listener !== 'function') {
|
|
return this;
|
|
}
|
|
if (!this._listeners) {
|
|
this._listeners = {};
|
|
}
|
|
if (!this._listeners[name]) {
|
|
this._listeners[name] = [];
|
|
}
|
|
this._listeners[name].push(listener);
|
|
return this;
|
|
};
|
|
/**
|
|
* Remove an event listener.
|
|
*/
|
|
World.prototype.off = function (name, listener) {
|
|
if (typeof name !== 'string' || typeof listener !== 'function') {
|
|
return this;
|
|
}
|
|
var listeners = this._listeners && this._listeners[name];
|
|
if (!listeners || !listeners.length) {
|
|
return this;
|
|
}
|
|
var index = listeners.indexOf(listener);
|
|
if (index >= 0) {
|
|
listeners.splice(index, 1);
|
|
}
|
|
return this;
|
|
};
|
|
World.prototype.publish = function (name, arg1, arg2, arg3) {
|
|
var listeners = this._listeners && this._listeners[name];
|
|
if (!listeners || !listeners.length) {
|
|
return 0;
|
|
}
|
|
for (var l = 0; l < listeners.length; l++) {
|
|
listeners[l].call(this, arg1, arg2, arg3);
|
|
}
|
|
return listeners.length;
|
|
};
|
|
/**
|
|
* @event World#remove-body
|
|
* @event World#remove-joint
|
|
* @event World#remove-fixture
|
|
*
|
|
* Joints and fixtures are destroyed when their associated body is destroyed.
|
|
* Register a destruction listener so that you may nullify references to these
|
|
* joints and shapes.
|
|
*
|
|
* `function(object)` is called when any joint or fixture is about to
|
|
* be destroyed due to the destruction of one of its attached or parent bodies.
|
|
*/
|
|
/**
|
|
* @internal
|
|
*/
|
|
World.prototype.beginContact = function (contact) {
|
|
this.publish('begin-contact', contact);
|
|
};
|
|
/**
|
|
* @event World#begin-contact
|
|
*
|
|
* Called when two fixtures begin to touch.
|
|
*
|
|
* Implement contact callbacks to get contact information. You can use these
|
|
* results for things like sounds and game logic. You can also get contact
|
|
* results by traversing the contact lists after the time step. However, you
|
|
* might miss some contacts because continuous physics leads to sub-stepping.
|
|
* Additionally you may receive multiple callbacks for the same contact in a
|
|
* single time step. You should strive to make your callbacks efficient because
|
|
* there may be many callbacks per time step.
|
|
*
|
|
* Warning: You cannot create/destroy world entities inside these callbacks.
|
|
*/
|
|
/**
|
|
* @internal
|
|
*/
|
|
World.prototype.endContact = function (contact) {
|
|
this.publish('end-contact', contact);
|
|
};
|
|
/**
|
|
* @event World#end-contact
|
|
*
|
|
* Called when two fixtures cease to touch.
|
|
*
|
|
* Implement contact callbacks to get contact information. You can use these
|
|
* results for things like sounds and game logic. You can also get contact
|
|
* results by traversing the contact lists after the time step. However, you
|
|
* might miss some contacts because continuous physics leads to sub-stepping.
|
|
* Additionally you may receive multiple callbacks for the same contact in a
|
|
* single time step. You should strive to make your callbacks efficient because
|
|
* there may be many callbacks per time step.
|
|
*
|
|
* Warning: You cannot create/destroy world entities inside these callbacks.
|
|
*/
|
|
/**
|
|
* @internal
|
|
*/
|
|
World.prototype.preSolve = function (contact, oldManifold) {
|
|
this.publish('pre-solve', contact, oldManifold);
|
|
};
|
|
/**
|
|
* @event World#pre-solve
|
|
*
|
|
* This is called after a contact is updated. This allows you to inspect a
|
|
* contact before it goes to the solver. If you are careful, you can modify the
|
|
* contact manifold (e.g. disable contact). A copy of the old manifold is
|
|
* provided so that you can detect changes. Note: this is called only for awake
|
|
* bodies. Note: this is called even when the number of contact points is zero.
|
|
* Note: this is not called for sensors. Note: if you set the number of contact
|
|
* points to zero, you will not get an endContact callback. However, you may get
|
|
* a beginContact callback the next step.
|
|
*
|
|
* Warning: You cannot create/destroy world entities inside these callbacks.
|
|
*/
|
|
/**
|
|
* @internal
|
|
*/
|
|
World.prototype.postSolve = function (contact, impulse) {
|
|
this.publish('post-solve', contact, impulse);
|
|
};
|
|
return World;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var Vec3 = /** @class */ (function () {
|
|
function Vec3(x, y, z) {
|
|
if (!(this instanceof Vec3)) {
|
|
return new Vec3(x, y, z);
|
|
}
|
|
if (typeof x === 'undefined') {
|
|
this.x = 0;
|
|
this.y = 0;
|
|
this.z = 0;
|
|
}
|
|
else if (typeof x === 'object') {
|
|
this.x = x.x;
|
|
this.y = x.y;
|
|
this.z = x.z;
|
|
}
|
|
else {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
}
|
|
}
|
|
/** @internal */
|
|
Vec3.prototype._serialize = function () {
|
|
return {
|
|
x: this.x,
|
|
y: this.y,
|
|
z: this.z
|
|
};
|
|
};
|
|
/** @internal */
|
|
Vec3._deserialize = function (data) {
|
|
var obj = Object.create(Vec3.prototype);
|
|
obj.x = data.x;
|
|
obj.y = data.y;
|
|
obj.z = data.z;
|
|
return obj;
|
|
};
|
|
/** @internal */
|
|
Vec3.neo = function (x, y, z) {
|
|
var obj = Object.create(Vec3.prototype);
|
|
obj.x = x;
|
|
obj.y = y;
|
|
obj.z = z;
|
|
return obj;
|
|
};
|
|
Vec3.zero = function () {
|
|
var obj = Object.create(Vec3.prototype);
|
|
obj.x = 0;
|
|
obj.y = 0;
|
|
obj.z = 0;
|
|
return obj;
|
|
};
|
|
Vec3.clone = function (v) {
|
|
return Vec3.neo(v.x, v.y, v.z);
|
|
};
|
|
Vec3.prototype.toString = function () {
|
|
return JSON.stringify(this);
|
|
};
|
|
/**
|
|
* Does this vector contain finite coordinates?
|
|
*/
|
|
Vec3.isValid = function (v) {
|
|
return v && math.isFinite(v.x) && math.isFinite(v.y) && math.isFinite(v.z);
|
|
};
|
|
Vec3.assert = function (o) {
|
|
return;
|
|
};
|
|
Vec3.prototype.setZero = function () {
|
|
this.x = 0.0;
|
|
this.y = 0.0;
|
|
this.z = 0.0;
|
|
return this;
|
|
};
|
|
Vec3.prototype.set = function (x, y, z) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
return this;
|
|
};
|
|
Vec3.prototype.add = function (w) {
|
|
this.x += w.x;
|
|
this.y += w.y;
|
|
this.z += w.z;
|
|
return this;
|
|
};
|
|
Vec3.prototype.sub = function (w) {
|
|
this.x -= w.x;
|
|
this.y -= w.y;
|
|
this.z -= w.z;
|
|
return this;
|
|
};
|
|
Vec3.prototype.mul = function (m) {
|
|
this.x *= m;
|
|
this.y *= m;
|
|
this.z *= m;
|
|
return this;
|
|
};
|
|
Vec3.areEqual = function (v, w) {
|
|
return v === w ||
|
|
typeof v === 'object' && v !== null &&
|
|
typeof w === 'object' && w !== null &&
|
|
v.x === w.x && v.y === w.y && v.z === w.z;
|
|
};
|
|
/**
|
|
* Perform the dot product on two vectors.
|
|
*/
|
|
Vec3.dot = function (v, w) {
|
|
return v.x * w.x + v.y * w.y + v.z * w.z;
|
|
};
|
|
/**
|
|
* Perform the cross product on two vectors. In 2D this produces a scalar.
|
|
*/
|
|
Vec3.cross = function (v, w) {
|
|
return new Vec3(v.y * w.z - v.z * w.y, v.z * w.x - v.x * w.z, v.x * w.y - v.y * w.x);
|
|
};
|
|
Vec3.add = function (v, w) {
|
|
return new Vec3(v.x + w.x, v.y + w.y, v.z + w.z);
|
|
};
|
|
Vec3.sub = function (v, w) {
|
|
return new Vec3(v.x - w.x, v.y - w.y, v.z - w.z);
|
|
};
|
|
Vec3.mul = function (v, m) {
|
|
return new Vec3(m * v.x, m * v.y, m * v.z);
|
|
};
|
|
Vec3.prototype.neg = function () {
|
|
this.x = -this.x;
|
|
this.y = -this.y;
|
|
this.z = -this.z;
|
|
return this;
|
|
};
|
|
Vec3.neg = function (v) {
|
|
return new Vec3(-v.x, -v.y, -v.z);
|
|
};
|
|
return Vec3;
|
|
}());
|
|
|
|
var SID = 0;
|
|
function Serializer(opts) {
|
|
opts = opts || {};
|
|
var rootClass = opts.rootClass || World;
|
|
var preSerialize = opts.preSerialize || function (obj) { return obj; };
|
|
var postSerialize = opts.postSerialize || function (data, obj) { return data; };
|
|
var preDeserialize = opts.preDeserialize || function (data) { return data; };
|
|
var postDeserialize = opts.postDeserialize || function (obj, data) { return obj; };
|
|
// This is used to create ref objects during serialize
|
|
var refTypes = {
|
|
World: World,
|
|
Body: Body,
|
|
Joint: Joint,
|
|
Fixture: Fixture,
|
|
Shape: Shape,
|
|
};
|
|
// This is used by restore to deserialize objects and refs
|
|
var restoreTypes = __assign({ Vec2: Vec2,
|
|
Vec3: Vec3 }, refTypes);
|
|
this.toJson = function (root) {
|
|
var json = [];
|
|
var queue = [root];
|
|
var refMap = {};
|
|
function storeRef(value, typeName) {
|
|
value.__sid = value.__sid || ++SID;
|
|
if (!refMap[value.__sid]) {
|
|
queue.push(value);
|
|
var index = json.length + queue.length;
|
|
var ref = {
|
|
refIndex: index,
|
|
refType: typeName
|
|
};
|
|
refMap[value.__sid] = ref;
|
|
}
|
|
return refMap[value.__sid];
|
|
}
|
|
function serialize(obj) {
|
|
obj = preSerialize(obj);
|
|
var data = obj._serialize();
|
|
data = postSerialize(data, obj);
|
|
return data;
|
|
}
|
|
function toJson(value, top) {
|
|
if (typeof value !== 'object' || value === null) {
|
|
return value;
|
|
}
|
|
if (typeof value._serialize === 'function') {
|
|
if (value !== top) {
|
|
for (var typeName in refTypes) {
|
|
if (value instanceof refTypes[typeName]) {
|
|
return storeRef(value, typeName);
|
|
}
|
|
}
|
|
}
|
|
value = serialize(value);
|
|
}
|
|
if (Array.isArray(value)) {
|
|
var newValue = [];
|
|
for (var key = 0; key < value.length; key++) {
|
|
newValue[key] = toJson(value[key]);
|
|
}
|
|
value = newValue;
|
|
}
|
|
else {
|
|
var newValue = {};
|
|
for (var key in value) {
|
|
if (value.hasOwnProperty(key)) {
|
|
newValue[key] = toJson(value[key]);
|
|
}
|
|
}
|
|
value = newValue;
|
|
}
|
|
return value;
|
|
}
|
|
while (queue.length) {
|
|
var obj = queue.shift();
|
|
var str = toJson(obj, obj);
|
|
json.push(str);
|
|
}
|
|
return json;
|
|
};
|
|
this.fromJson = function (json) {
|
|
var refMap = {};
|
|
function deserialize(cls, data, ctx) {
|
|
data = preDeserialize(data);
|
|
var obj = cls._deserialize(data, ctx, restoreRef);
|
|
obj = postDeserialize(obj, data);
|
|
return obj;
|
|
}
|
|
function restoreRef(cls, ref, ctx) {
|
|
if (!ref.refIndex) {
|
|
return cls && cls._deserialize && deserialize(cls, ref, ctx);
|
|
}
|
|
cls = restoreTypes[ref.refType] || cls;
|
|
var index = ref.refIndex;
|
|
if (!refMap[index]) {
|
|
var data = json[index];
|
|
var obj = deserialize(cls, data, ctx);
|
|
refMap[index] = obj;
|
|
}
|
|
return refMap[index];
|
|
}
|
|
var root = rootClass._deserialize(json[0], null, restoreRef);
|
|
return root;
|
|
};
|
|
}
|
|
var serializer = new Serializer();
|
|
Serializer.toJson = serializer.toJson;
|
|
Serializer.fromJson = serializer.fromJson;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A 3-by-3 matrix. Stored in column-major order.
|
|
*/
|
|
var Mat33 = /** @class */ (function () {
|
|
function Mat33(a, b, c) {
|
|
if (typeof a === 'object' && a !== null) {
|
|
this.ex = Vec3.clone(a);
|
|
this.ey = Vec3.clone(b);
|
|
this.ez = Vec3.clone(c);
|
|
}
|
|
else {
|
|
this.ex = Vec3.zero();
|
|
this.ey = Vec3.zero();
|
|
this.ez = Vec3.zero();
|
|
}
|
|
}
|
|
Mat33.prototype.toString = function () {
|
|
return JSON.stringify(this);
|
|
};
|
|
Mat33.isValid = function (o) {
|
|
return o && Vec3.isValid(o.ex) && Vec3.isValid(o.ey) && Vec3.isValid(o.ez);
|
|
};
|
|
Mat33.assert = function (o) {
|
|
return;
|
|
};
|
|
/**
|
|
* Set this matrix to all zeros.
|
|
*/
|
|
Mat33.prototype.setZero = function () {
|
|
this.ex.setZero();
|
|
this.ey.setZero();
|
|
this.ez.setZero();
|
|
return this;
|
|
};
|
|
/**
|
|
* Solve A * x = b, where b is a column vector. This is more efficient than
|
|
* computing the inverse in one-shot cases.
|
|
*/
|
|
Mat33.prototype.solve33 = function (v) {
|
|
var det = Vec3.dot(this.ex, Vec3.cross(this.ey, this.ez));
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
var r = new Vec3();
|
|
r.x = det * Vec3.dot(v, Vec3.cross(this.ey, this.ez));
|
|
r.y = det * Vec3.dot(this.ex, Vec3.cross(v, this.ez));
|
|
r.z = det * Vec3.dot(this.ex, Vec3.cross(this.ey, v));
|
|
return r;
|
|
};
|
|
/**
|
|
* Solve A * x = b, where b is a column vector. This is more efficient than
|
|
* computing the inverse in one-shot cases. Solve only the upper 2-by-2 matrix
|
|
* equation.
|
|
*/
|
|
Mat33.prototype.solve22 = function (v) {
|
|
var a11 = this.ex.x;
|
|
var a12 = this.ey.x;
|
|
var a21 = this.ex.y;
|
|
var a22 = this.ey.y;
|
|
var det = a11 * a22 - a12 * a21;
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
var r = Vec2.zero();
|
|
r.x = det * (a22 * v.x - a12 * v.y);
|
|
r.y = det * (a11 * v.y - a21 * v.x);
|
|
return r;
|
|
};
|
|
/**
|
|
* Get the inverse of this matrix as a 2-by-2. Returns the zero matrix if
|
|
* singular.
|
|
*/
|
|
Mat33.prototype.getInverse22 = function (M) {
|
|
var a = this.ex.x;
|
|
var b = this.ey.x;
|
|
var c = this.ex.y;
|
|
var d = this.ey.y;
|
|
var det = a * d - b * c;
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
M.ex.x = det * d;
|
|
M.ey.x = -det * b;
|
|
M.ex.z = 0.0;
|
|
M.ex.y = -det * c;
|
|
M.ey.y = det * a;
|
|
M.ey.z = 0.0;
|
|
M.ez.x = 0.0;
|
|
M.ez.y = 0.0;
|
|
M.ez.z = 0.0;
|
|
};
|
|
/**
|
|
* Get the symmetric inverse of this matrix as a 3-by-3. Returns the zero matrix
|
|
* if singular.
|
|
*/
|
|
Mat33.prototype.getSymInverse33 = function (M) {
|
|
var det = Vec3.dot(this.ex, Vec3.cross(this.ey, this.ez));
|
|
if (det !== 0.0) {
|
|
det = 1.0 / det;
|
|
}
|
|
var a11 = this.ex.x;
|
|
var a12 = this.ey.x;
|
|
var a13 = this.ez.x;
|
|
var a22 = this.ey.y;
|
|
var a23 = this.ez.y;
|
|
var a33 = this.ez.z;
|
|
M.ex.x = det * (a22 * a33 - a23 * a23);
|
|
M.ex.y = det * (a13 * a23 - a12 * a33);
|
|
M.ex.z = det * (a12 * a23 - a13 * a22);
|
|
M.ey.x = M.ex.y;
|
|
M.ey.y = det * (a11 * a33 - a13 * a13);
|
|
M.ey.z = det * (a13 * a12 - a11 * a23);
|
|
M.ez.x = M.ex.z;
|
|
M.ez.y = M.ey.z;
|
|
M.ez.z = det * (a11 * a22 - a12 * a12);
|
|
};
|
|
Mat33.mul = function (a, b) {
|
|
if (b && 'z' in b && 'y' in b && 'x' in b) {
|
|
var x = a.ex.x * b.x + a.ey.x * b.y + a.ez.x * b.z;
|
|
var y = a.ex.y * b.x + a.ey.y * b.y + a.ez.y * b.z;
|
|
var z = a.ex.z * b.x + a.ey.z * b.y + a.ez.z * b.z;
|
|
return new Vec3(x, y, z);
|
|
}
|
|
else if (b && 'y' in b && 'x' in b) {
|
|
var x = a.ex.x * b.x + a.ey.x * b.y;
|
|
var y = a.ex.y * b.x + a.ey.y * b.y;
|
|
return Vec2.neo(x, y);
|
|
}
|
|
};
|
|
Mat33.mulVec3 = function (a, b) {
|
|
var x = a.ex.x * b.x + a.ey.x * b.y + a.ez.x * b.z;
|
|
var y = a.ex.y * b.x + a.ey.y * b.y + a.ez.y * b.z;
|
|
var z = a.ex.z * b.x + a.ey.z * b.y + a.ez.z * b.z;
|
|
return new Vec3(x, y, z);
|
|
};
|
|
Mat33.mulVec2 = function (a, b) {
|
|
var x = a.ex.x * b.x + a.ey.x * b.y;
|
|
var y = a.ex.y * b.x + a.ey.y * b.y;
|
|
return Vec2.neo(x, y);
|
|
};
|
|
Mat33.add = function (a, b) {
|
|
return new Mat33(Vec3.add(a.ex, b.ex), Vec3.add(a.ey, b.ey), Vec3.add(a.ez, b.ez));
|
|
};
|
|
return Mat33;
|
|
}());
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var CircleShape = /** @class */ (function (_super) {
|
|
__extends(CircleShape, _super);
|
|
function CircleShape(a, b) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof CircleShape)) {
|
|
return new CircleShape(a, b);
|
|
}
|
|
_this = _super.call(this) || this;
|
|
_this.m_type = CircleShape.TYPE;
|
|
_this.m_p = Vec2.zero();
|
|
_this.m_radius = 1;
|
|
if (typeof a === 'object' && Vec2.isValid(a)) {
|
|
_this.m_p.set(a);
|
|
if (typeof b === 'number') {
|
|
_this.m_radius = b;
|
|
}
|
|
}
|
|
else if (typeof a === 'number') {
|
|
_this.m_radius = a;
|
|
}
|
|
return _this;
|
|
}
|
|
/** @internal */
|
|
CircleShape.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
p: this.m_p,
|
|
radius: this.m_radius,
|
|
};
|
|
};
|
|
/** @internal */
|
|
CircleShape._deserialize = function (data) {
|
|
return new CircleShape(data.p, data.radius);
|
|
};
|
|
// TODO: already defined in Shape
|
|
CircleShape.prototype.getRadius = function () {
|
|
return this.m_radius;
|
|
};
|
|
CircleShape.prototype.getCenter = function () {
|
|
return this.m_p;
|
|
};
|
|
CircleShape.prototype.getVertex = function (index) {
|
|
return this.m_p;
|
|
};
|
|
/**
|
|
* @deprecated Shapes should be treated as immutable.
|
|
*
|
|
* clone the concrete shape.
|
|
*/
|
|
CircleShape.prototype._clone = function () {
|
|
var clone = new CircleShape();
|
|
clone.m_type = this.m_type;
|
|
clone.m_radius = this.m_radius;
|
|
clone.m_p = this.m_p.clone();
|
|
return clone;
|
|
};
|
|
/**
|
|
* Get the number of child primitives.
|
|
*/
|
|
CircleShape.prototype.getChildCount = function () {
|
|
return 1;
|
|
};
|
|
/**
|
|
* Test a point for containment in this shape. This only works for convex
|
|
* shapes.
|
|
*
|
|
* @param {Transform} xf The shape world transform.
|
|
* @param p A point in world coordinates.
|
|
*/
|
|
CircleShape.prototype.testPoint = function (xf, p) {
|
|
var center = Vec2.add(xf.p, Rot.mulVec2(xf.q, this.m_p));
|
|
var d = Vec2.sub(p, center);
|
|
return Vec2.dot(d, d) <= this.m_radius * this.m_radius;
|
|
};
|
|
/**
|
|
* Cast a ray against a child shape.
|
|
*
|
|
* @param {RayCastOutput} output The ray-cast results.
|
|
* @param {RayCastInput} input The ray-cast input parameters.
|
|
* @param {Transform} transform The transform to be applied to the shape.
|
|
* @param childIndex The child shape index
|
|
*/
|
|
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
|
|
// From Section 3.1.2
|
|
// x = s + a * r
|
|
// norm(x) = radius
|
|
CircleShape.prototype.rayCast = function (output, input, xf, childIndex) {
|
|
var position = Vec2.add(xf.p, Rot.mulVec2(xf.q, this.m_p));
|
|
var s = Vec2.sub(input.p1, position);
|
|
var b = Vec2.dot(s, s) - this.m_radius * this.m_radius;
|
|
// Solve quadratic equation.
|
|
var r = Vec2.sub(input.p2, input.p1);
|
|
var c = Vec2.dot(s, r);
|
|
var rr = Vec2.dot(r, r);
|
|
var sigma = c * c - rr * b;
|
|
// Check for negative discriminant and short segment.
|
|
if (sigma < 0.0 || rr < math.EPSILON) {
|
|
return false;
|
|
}
|
|
// Find the point of intersection of the line with the circle.
|
|
var a = -(c + math.sqrt(sigma));
|
|
// Is the intersection point on the segment?
|
|
if (0.0 <= a && a <= input.maxFraction * rr) {
|
|
a /= rr;
|
|
output.fraction = a;
|
|
output.normal = Vec2.add(s, Vec2.mul(a, r));
|
|
output.normal.normalize();
|
|
return true;
|
|
}
|
|
return false;
|
|
};
|
|
/**
|
|
* Given a transform, compute the associated axis aligned bounding box for a
|
|
* child shape.
|
|
*
|
|
* @param {AABB} aabb Returns the axis aligned box.
|
|
* @param {Transform} xf The world transform of the shape.
|
|
* @param childIndex The child shape
|
|
*/
|
|
CircleShape.prototype.computeAABB = function (aabb, xf, childIndex) {
|
|
var p = Vec2.add(xf.p, Rot.mulVec2(xf.q, this.m_p));
|
|
aabb.lowerBound.set(p.x - this.m_radius, p.y - this.m_radius);
|
|
aabb.upperBound.set(p.x + this.m_radius, p.y + this.m_radius);
|
|
};
|
|
/**
|
|
* Compute the mass properties of this shape using its dimensions and density.
|
|
* The inertia tensor is computed about the local origin.
|
|
*
|
|
* @param {MassData} massData Returns the mass data for this shape.
|
|
* @param density The density in kilograms per meter squared.
|
|
*/
|
|
CircleShape.prototype.computeMass = function (massData, density) {
|
|
massData.mass = density * math.PI * this.m_radius * this.m_radius;
|
|
massData.center = this.m_p;
|
|
// inertia about the local origin
|
|
massData.I = massData.mass
|
|
* (0.5 * this.m_radius * this.m_radius + Vec2.dot(this.m_p, this.m_p));
|
|
};
|
|
CircleShape.prototype.computeDistanceProxy = function (proxy) {
|
|
proxy.m_vertices.push(this.m_p);
|
|
proxy.m_count = 1;
|
|
proxy.m_radius = this.m_radius;
|
|
};
|
|
CircleShape.TYPE = 'circle';
|
|
return CircleShape;
|
|
}(Shape));
|
|
Shape.TYPES[CircleShape.TYPE] = CircleShape;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A line segment (edge) shape. These can be connected in chains or loops to
|
|
* other edge shapes. The connectivity information is used to ensure correct
|
|
* contact normals.
|
|
*/
|
|
var EdgeShape = /** @class */ (function (_super) {
|
|
__extends(EdgeShape, _super);
|
|
function EdgeShape(v1, v2) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof EdgeShape)) {
|
|
return new EdgeShape(v1, v2);
|
|
}
|
|
_this = _super.call(this) || this;
|
|
_this.m_type = EdgeShape.TYPE;
|
|
_this.m_radius = Settings.polygonRadius;
|
|
_this.m_vertex1 = v1 ? Vec2.clone(v1) : Vec2.zero();
|
|
_this.m_vertex2 = v2 ? Vec2.clone(v2) : Vec2.zero();
|
|
_this.m_vertex0 = Vec2.zero();
|
|
_this.m_vertex3 = Vec2.zero();
|
|
_this.m_hasVertex0 = false;
|
|
_this.m_hasVertex3 = false;
|
|
return _this;
|
|
}
|
|
/** @internal */
|
|
EdgeShape.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
vertex1: this.m_vertex1,
|
|
vertex2: this.m_vertex2,
|
|
vertex0: this.m_vertex0,
|
|
vertex3: this.m_vertex3,
|
|
hasVertex0: this.m_hasVertex0,
|
|
hasVertex3: this.m_hasVertex3,
|
|
};
|
|
};
|
|
/** @internal */
|
|
EdgeShape._deserialize = function (data) {
|
|
var shape = new EdgeShape(data.vertex1, data.vertex2);
|
|
if (shape.m_hasVertex0) {
|
|
shape.setPrev(data.vertex0);
|
|
}
|
|
if (shape.m_hasVertex3) {
|
|
shape.setNext(data.vertex3);
|
|
}
|
|
return shape;
|
|
};
|
|
EdgeShape.prototype.setNext = function (v3) {
|
|
if (v3) {
|
|
this.m_vertex3.set(v3);
|
|
this.m_hasVertex3 = true;
|
|
}
|
|
else {
|
|
this.m_vertex3.setZero();
|
|
this.m_hasVertex3 = false;
|
|
}
|
|
return this;
|
|
};
|
|
EdgeShape.prototype.setPrev = function (v0) {
|
|
if (v0) {
|
|
this.m_vertex0.set(v0);
|
|
this.m_hasVertex0 = true;
|
|
}
|
|
else {
|
|
this.m_vertex0.setZero();
|
|
this.m_hasVertex0 = false;
|
|
}
|
|
return this;
|
|
};
|
|
/**
|
|
* Set this as an isolated edge.
|
|
*/
|
|
EdgeShape.prototype._set = function (v1, v2) {
|
|
this.m_vertex1.set(v1);
|
|
this.m_vertex2.set(v2);
|
|
this.m_hasVertex0 = false;
|
|
this.m_hasVertex3 = false;
|
|
return this;
|
|
};
|
|
/**
|
|
* @deprecated Shapes should be treated as immutable.
|
|
*
|
|
* clone the concrete shape.
|
|
*/
|
|
EdgeShape.prototype._clone = function () {
|
|
var clone = new EdgeShape();
|
|
clone.m_type = this.m_type;
|
|
clone.m_radius = this.m_radius;
|
|
clone.m_vertex1.set(this.m_vertex1);
|
|
clone.m_vertex2.set(this.m_vertex2);
|
|
clone.m_vertex0.set(this.m_vertex0);
|
|
clone.m_vertex3.set(this.m_vertex3);
|
|
clone.m_hasVertex0 = this.m_hasVertex0;
|
|
clone.m_hasVertex3 = this.m_hasVertex3;
|
|
return clone;
|
|
};
|
|
/**
|
|
* Get the number of child primitives.
|
|
*/
|
|
EdgeShape.prototype.getChildCount = function () {
|
|
return 1;
|
|
};
|
|
/**
|
|
* Test a point for containment in this shape. This only works for convex
|
|
* shapes.
|
|
*
|
|
* @param {Transform} xf The shape world transform.
|
|
* @param p A point in world coordinates.
|
|
*/
|
|
EdgeShape.prototype.testPoint = function (xf, p) {
|
|
return false;
|
|
};
|
|
/**
|
|
* Cast a ray against a child shape.
|
|
*
|
|
* @param {RayCastOutput} output The ray-cast results.
|
|
* @param {RayCastInput} input The ray-cast input parameters.
|
|
* @param {Transform} transform The transform to be applied to the shape.
|
|
* @param childIndex The child shape index
|
|
*/
|
|
// p = p1 + t * d
|
|
// v = v1 + s * e
|
|
// p1 + t * d = v1 + s * e
|
|
// s * e - t * d = p1 - v1
|
|
EdgeShape.prototype.rayCast = function (output, input, xf, childIndex) {
|
|
// NOT_USED(childIndex);
|
|
// Put the ray into the edge's frame of reference.
|
|
var p1 = Rot.mulTVec2(xf.q, Vec2.sub(input.p1, xf.p));
|
|
var p2 = Rot.mulTVec2(xf.q, Vec2.sub(input.p2, xf.p));
|
|
var d = Vec2.sub(p2, p1);
|
|
var v1 = this.m_vertex1;
|
|
var v2 = this.m_vertex2;
|
|
var e = Vec2.sub(v2, v1);
|
|
var normal = Vec2.neo(e.y, -e.x);
|
|
normal.normalize();
|
|
// q = p1 + t * d
|
|
// dot(normal, q - v1) = 0
|
|
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
|
|
var numerator = Vec2.dot(normal, Vec2.sub(v1, p1));
|
|
var denominator = Vec2.dot(normal, d);
|
|
if (denominator == 0.0) {
|
|
return false;
|
|
}
|
|
var t = numerator / denominator;
|
|
if (t < 0.0 || input.maxFraction < t) {
|
|
return false;
|
|
}
|
|
var q = Vec2.add(p1, Vec2.mul(t, d));
|
|
// q = v1 + s * r
|
|
// s = dot(q - v1, r) / dot(r, r)
|
|
var r = Vec2.sub(v2, v1);
|
|
var rr = Vec2.dot(r, r);
|
|
if (rr == 0.0) {
|
|
return false;
|
|
}
|
|
var s = Vec2.dot(Vec2.sub(q, v1), r) / rr;
|
|
if (s < 0.0 || 1.0 < s) {
|
|
return false;
|
|
}
|
|
output.fraction = t;
|
|
if (numerator > 0.0) {
|
|
output.normal = Rot.mulVec2(xf.q, normal).neg();
|
|
}
|
|
else {
|
|
output.normal = Rot.mulVec2(xf.q, normal);
|
|
}
|
|
return true;
|
|
};
|
|
/**
|
|
* Given a transform, compute the associated axis aligned bounding box for a
|
|
* child shape.
|
|
*
|
|
* @param {AABB} aabb Returns the axis aligned box.
|
|
* @param {Transform} xf The world transform of the shape.
|
|
* @param childIndex The child shape
|
|
*/
|
|
EdgeShape.prototype.computeAABB = function (aabb, xf, childIndex) {
|
|
var v1 = Transform.mulVec2(xf, this.m_vertex1);
|
|
var v2 = Transform.mulVec2(xf, this.m_vertex2);
|
|
aabb.combinePoints(v1, v2);
|
|
aabb.extend(this.m_radius);
|
|
};
|
|
/**
|
|
* Compute the mass properties of this shape using its dimensions and density.
|
|
* The inertia tensor is computed about the local origin.
|
|
*
|
|
* @param {MassData} massData Returns the mass data for this shape.
|
|
* @param density The density in kilograms per meter squared.
|
|
*/
|
|
EdgeShape.prototype.computeMass = function (massData, density) {
|
|
massData.mass = 0.0;
|
|
massData.center.setCombine(0.5, this.m_vertex1, 0.5, this.m_vertex2);
|
|
massData.I = 0.0;
|
|
};
|
|
EdgeShape.prototype.computeDistanceProxy = function (proxy) {
|
|
proxy.m_vertices.push(this.m_vertex1);
|
|
proxy.m_vertices.push(this.m_vertex2);
|
|
proxy.m_count = 2;
|
|
proxy.m_radius = this.m_radius;
|
|
};
|
|
EdgeShape.TYPE = 'edge';
|
|
return EdgeShape;
|
|
}(Shape));
|
|
Shape.TYPES[EdgeShape.TYPE] = EdgeShape;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A convex polygon. It is assumed that the interior of the polygon is to the
|
|
* left of each edge. Polygons have a maximum number of vertices equal to
|
|
* Settings.maxPolygonVertices. In most cases you should not need many vertices
|
|
* for a convex polygon. extends Shape
|
|
*/
|
|
var PolygonShape = /** @class */ (function (_super) {
|
|
__extends(PolygonShape, _super);
|
|
// @ts-ignore
|
|
function PolygonShape(vertices) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof PolygonShape)) {
|
|
return new PolygonShape(vertices);
|
|
}
|
|
_this = _super.call(this) || this;
|
|
_this.m_type = PolygonShape.TYPE;
|
|
_this.m_radius = Settings.polygonRadius;
|
|
_this.m_centroid = Vec2.zero();
|
|
_this.m_vertices = []; // Vec2[Settings.maxPolygonVertices]
|
|
_this.m_normals = []; // Vec2[Settings.maxPolygonVertices]
|
|
_this.m_count = 0;
|
|
if (vertices && vertices.length) {
|
|
_this._set(vertices);
|
|
}
|
|
return _this;
|
|
}
|
|
/** @internal */
|
|
PolygonShape.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
vertices: this.m_vertices,
|
|
};
|
|
};
|
|
/** @internal */
|
|
PolygonShape._deserialize = function (data, fixture, restore) {
|
|
var vertices = [];
|
|
if (data.vertices) {
|
|
for (var i = 0; i < data.vertices.length; i++) {
|
|
vertices.push(restore(Vec2, data.vertices[i]));
|
|
}
|
|
}
|
|
var shape = new PolygonShape(vertices);
|
|
return shape;
|
|
};
|
|
PolygonShape.prototype.getVertex = function (index) {
|
|
return this.m_vertices[index];
|
|
};
|
|
/**
|
|
* @deprecated Shapes should be treated as immutable.
|
|
*
|
|
* clone the concrete shape.
|
|
*/
|
|
PolygonShape.prototype._clone = function () {
|
|
var clone = new PolygonShape();
|
|
clone.m_type = this.m_type;
|
|
clone.m_radius = this.m_radius;
|
|
clone.m_count = this.m_count;
|
|
clone.m_centroid.set(this.m_centroid);
|
|
for (var i = 0; i < this.m_count; i++) {
|
|
clone.m_vertices.push(this.m_vertices[i].clone());
|
|
}
|
|
for (var i = 0; i < this.m_normals.length; i++) {
|
|
clone.m_normals.push(this.m_normals[i].clone());
|
|
}
|
|
return clone;
|
|
};
|
|
/**
|
|
* Get the number of child primitives.
|
|
*/
|
|
PolygonShape.prototype.getChildCount = function () {
|
|
return 1;
|
|
};
|
|
PolygonShape.prototype._reset = function () {
|
|
this._set(this.m_vertices);
|
|
};
|
|
/**
|
|
* @internal
|
|
*
|
|
* Create a convex hull from the given array of local points. The count must be
|
|
* in the range [3, Settings.maxPolygonVertices].
|
|
*
|
|
* Warning: the points may be re-ordered, even if they form a convex polygon
|
|
* Warning: collinear points are handled but not removed. Collinear points may
|
|
* lead to poor stacking behavior.
|
|
*/
|
|
PolygonShape.prototype._set = function (vertices) {
|
|
if (vertices.length < 3) {
|
|
this._setAsBox(1.0, 1.0);
|
|
return;
|
|
}
|
|
var n = math.min(vertices.length, Settings.maxPolygonVertices);
|
|
// Perform welding and copy vertices into local buffer.
|
|
var ps = []; // [Settings.maxPolygonVertices];
|
|
for (var i = 0; i < n; ++i) {
|
|
var v = vertices[i];
|
|
var unique = true;
|
|
for (var j = 0; j < ps.length; ++j) {
|
|
if (Vec2.distanceSquared(v, ps[j]) < 0.25 * Settings.linearSlopSquared) {
|
|
unique = false;
|
|
break;
|
|
}
|
|
}
|
|
if (unique) {
|
|
ps.push(v);
|
|
}
|
|
}
|
|
n = ps.length;
|
|
if (n < 3) {
|
|
this._setAsBox(1.0, 1.0);
|
|
return;
|
|
}
|
|
// Create the convex hull using the Gift wrapping algorithm
|
|
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
|
|
// Find the right most point on the hull (in case of multiple points bottom most is used)
|
|
var i0 = 0;
|
|
var x0 = ps[0].x;
|
|
for (var i = 1; i < n; ++i) {
|
|
var x = ps[i].x;
|
|
if (x > x0 || (x === x0 && ps[i].y < ps[i0].y)) {
|
|
i0 = i;
|
|
x0 = x;
|
|
}
|
|
}
|
|
var hull = []; // [Settings.maxPolygonVertices];
|
|
var m = 0;
|
|
var ih = i0;
|
|
while (true) {
|
|
hull[m] = ih;
|
|
var ie = 0;
|
|
for (var j = 1; j < n; ++j) {
|
|
if (ie === ih) {
|
|
ie = j;
|
|
continue;
|
|
}
|
|
var r = Vec2.sub(ps[ie], ps[hull[m]]);
|
|
var v = Vec2.sub(ps[j], ps[hull[m]]);
|
|
var c = Vec2.cross(r, v);
|
|
// c < 0 means counter-clockwise wrapping, c > 0 means clockwise wrapping
|
|
if (c < 0.0) {
|
|
ie = j;
|
|
}
|
|
// Collinearity check
|
|
if (c === 0.0 && v.lengthSquared() > r.lengthSquared()) {
|
|
ie = j;
|
|
}
|
|
}
|
|
++m;
|
|
ih = ie;
|
|
if (ie === i0) {
|
|
break;
|
|
}
|
|
}
|
|
if (m < 3) {
|
|
this._setAsBox(1.0, 1.0);
|
|
return;
|
|
}
|
|
this.m_count = m;
|
|
// Copy vertices.
|
|
this.m_vertices = [];
|
|
for (var i = 0; i < m; ++i) {
|
|
this.m_vertices[i] = ps[hull[i]];
|
|
}
|
|
// Compute normals. Ensure the edges have non-zero length.
|
|
for (var i = 0; i < m; ++i) {
|
|
var i1 = i;
|
|
var i2 = i + 1 < m ? i + 1 : 0;
|
|
var edge = Vec2.sub(this.m_vertices[i2], this.m_vertices[i1]);
|
|
this.m_normals[i] = Vec2.cross(edge, 1.0);
|
|
this.m_normals[i].normalize();
|
|
}
|
|
// Compute the polygon centroid.
|
|
this.m_centroid = ComputeCentroid(this.m_vertices, m);
|
|
};
|
|
PolygonShape.prototype._setAsBox = function (hx, hy, center, angle) {
|
|
// start with right-bottom, counter-clockwise, as in Gift wrapping algorithm in PolygonShape._set()
|
|
this.m_vertices[0] = Vec2.neo(hx, -hy);
|
|
this.m_vertices[1] = Vec2.neo(hx, hy);
|
|
this.m_vertices[2] = Vec2.neo(-hx, hy);
|
|
this.m_vertices[3] = Vec2.neo(-hx, -hy);
|
|
this.m_normals[0] = Vec2.neo(1.0, 0.0);
|
|
this.m_normals[1] = Vec2.neo(0.0, 1.0);
|
|
this.m_normals[2] = Vec2.neo(-1.0, 0.0);
|
|
this.m_normals[3] = Vec2.neo(0.0, -1.0);
|
|
this.m_count = 4;
|
|
if (Vec2.isValid(center)) {
|
|
angle = angle || 0;
|
|
this.m_centroid.set(center);
|
|
var xf = Transform.identity();
|
|
xf.p.set(center);
|
|
xf.q.set(angle);
|
|
// Transform vertices and normals.
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
this.m_vertices[i] = Transform.mulVec2(xf, this.m_vertices[i]);
|
|
this.m_normals[i] = Rot.mulVec2(xf.q, this.m_normals[i]);
|
|
}
|
|
}
|
|
};
|
|
/**
|
|
* Test a point for containment in this shape. This only works for convex
|
|
* shapes.
|
|
*
|
|
* @param {Transform} xf The shape world transform.
|
|
* @param p A point in world coordinates.
|
|
*/
|
|
PolygonShape.prototype.testPoint = function (xf, p) {
|
|
var pLocal = Rot.mulTVec2(xf.q, Vec2.sub(p, xf.p));
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
var dot = Vec2.dot(this.m_normals[i], Vec2.sub(pLocal, this.m_vertices[i]));
|
|
if (dot > 0.0) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
};
|
|
/**
|
|
* Cast a ray against a child shape.
|
|
*
|
|
* @param output The ray-cast results.
|
|
* @param input The ray-cast input parameters.
|
|
* @param xf The transform to be applied to the shape.
|
|
* @param childIndex The child shape index
|
|
*/
|
|
PolygonShape.prototype.rayCast = function (output, input, xf, childIndex) {
|
|
// Put the ray into the polygon's frame of reference.
|
|
var p1 = Rot.mulTVec2(xf.q, Vec2.sub(input.p1, xf.p));
|
|
var p2 = Rot.mulTVec2(xf.q, Vec2.sub(input.p2, xf.p));
|
|
var d = Vec2.sub(p2, p1);
|
|
var lower = 0.0;
|
|
var upper = input.maxFraction;
|
|
var index = -1;
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
// p = p1 + a * d
|
|
// dot(normal, p - v) = 0
|
|
// dot(normal, p1 - v) + a * dot(normal, d) = 0
|
|
var numerator = Vec2.dot(this.m_normals[i], Vec2.sub(this.m_vertices[i], p1));
|
|
var denominator = Vec2.dot(this.m_normals[i], d);
|
|
if (denominator == 0.0) {
|
|
if (numerator < 0.0) {
|
|
return false;
|
|
}
|
|
}
|
|
else {
|
|
// Note: we want this predicate without division:
|
|
// lower < numerator / denominator, where denominator < 0
|
|
// Since denominator < 0, we have to flip the inequality:
|
|
// lower < numerator / denominator <==> denominator * lower > numerator.
|
|
if (denominator < 0.0 && numerator < lower * denominator) {
|
|
// Increase lower.
|
|
// The segment enters this half-space.
|
|
lower = numerator / denominator;
|
|
index = i;
|
|
}
|
|
else if (denominator > 0.0 && numerator < upper * denominator) {
|
|
// Decrease upper.
|
|
// The segment exits this half-space.
|
|
upper = numerator / denominator;
|
|
}
|
|
}
|
|
// The use of epsilon here causes the assert on lower to trip
|
|
// in some cases. Apparently the use of epsilon was to make edge
|
|
// shapes work, but now those are handled separately.
|
|
// if (upper < lower - Math.EPSILON)
|
|
if (upper < lower) {
|
|
return false;
|
|
}
|
|
}
|
|
if (index >= 0) {
|
|
output.fraction = lower;
|
|
output.normal = Rot.mulVec2(xf.q, this.m_normals[index]);
|
|
return true;
|
|
}
|
|
return false;
|
|
};
|
|
/**
|
|
* Given a transform, compute the associated axis aligned bounding box for a
|
|
* child shape.
|
|
*
|
|
* @param aabb Returns the axis aligned box.
|
|
* @param xf The world transform of the shape.
|
|
* @param childIndex The child shape
|
|
*/
|
|
PolygonShape.prototype.computeAABB = function (aabb, xf, childIndex) {
|
|
var minX = Infinity;
|
|
var minY = Infinity;
|
|
var maxX = -Infinity;
|
|
var maxY = -Infinity;
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
var v = Transform.mulVec2(xf, this.m_vertices[i]);
|
|
minX = math.min(minX, v.x);
|
|
maxX = math.max(maxX, v.x);
|
|
minY = math.min(minY, v.y);
|
|
maxY = math.max(maxY, v.y);
|
|
}
|
|
aabb.lowerBound.set(minX, minY);
|
|
aabb.upperBound.set(maxX, maxY);
|
|
aabb.extend(this.m_radius);
|
|
};
|
|
/**
|
|
* Compute the mass properties of this shape using its dimensions and density.
|
|
* The inertia tensor is computed about the local origin.
|
|
*
|
|
* @param {MassData} massData Returns the mass data for this shape.
|
|
* @param density The density in kilograms per meter squared.
|
|
*/
|
|
PolygonShape.prototype.computeMass = function (massData, density) {
|
|
var center = Vec2.zero();
|
|
var area = 0.0;
|
|
var I = 0.0;
|
|
// s is the reference point for forming triangles.
|
|
// It's location doesn't change the result (except for rounding error).
|
|
var s = Vec2.zero();
|
|
// This code would put the reference point inside the polygon.
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
s.add(this.m_vertices[i]);
|
|
}
|
|
s.mul(1.0 / this.m_count);
|
|
var k_inv3 = 1.0 / 3.0;
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
// Triangle vertices.
|
|
var e1 = Vec2.sub(this.m_vertices[i], s);
|
|
var e2 = i + 1 < this.m_count ? Vec2.sub(this.m_vertices[i + 1], s) : Vec2.sub(this.m_vertices[0], s);
|
|
var D = Vec2.cross(e1, e2);
|
|
var triangleArea = 0.5 * D;
|
|
area += triangleArea;
|
|
// Area weighted centroid
|
|
center.addCombine(triangleArea * k_inv3, e1, triangleArea * k_inv3, e2);
|
|
var ex1 = e1.x;
|
|
var ey1 = e1.y;
|
|
var ex2 = e2.x;
|
|
var ey2 = e2.y;
|
|
var intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
|
|
var inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
|
|
I += (0.25 * k_inv3 * D) * (intx2 + inty2);
|
|
}
|
|
// Total mass
|
|
massData.mass = density * area;
|
|
center.mul(1.0 / area);
|
|
massData.center.setCombine(1, center, 1, s);
|
|
// Inertia tensor relative to the local origin (point s).
|
|
massData.I = density * I;
|
|
// Shift to center of mass then to original body origin.
|
|
massData.I += massData.mass * (Vec2.dot(massData.center, massData.center) - Vec2.dot(center, center));
|
|
};
|
|
/**
|
|
* Validate convexity. This is a very time consuming operation.
|
|
* @returns true if valid
|
|
*/
|
|
PolygonShape.prototype.validate = function () {
|
|
for (var i = 0; i < this.m_count; ++i) {
|
|
var i1 = i;
|
|
var i2 = i < this.m_count - 1 ? i1 + 1 : 0;
|
|
var p = this.m_vertices[i1];
|
|
var e = Vec2.sub(this.m_vertices[i2], p);
|
|
for (var j = 0; j < this.m_count; ++j) {
|
|
if (j == i1 || j == i2) {
|
|
continue;
|
|
}
|
|
var v = Vec2.sub(this.m_vertices[j], p);
|
|
var c = Vec2.cross(e, v);
|
|
if (c < 0.0) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
};
|
|
PolygonShape.prototype.computeDistanceProxy = function (proxy) {
|
|
proxy.m_vertices = this.m_vertices;
|
|
proxy.m_count = this.m_count;
|
|
proxy.m_radius = this.m_radius;
|
|
};
|
|
PolygonShape.TYPE = 'polygon';
|
|
return PolygonShape;
|
|
}(Shape));
|
|
function ComputeCentroid(vs, count) {
|
|
var c = Vec2.zero();
|
|
var area = 0.0;
|
|
// pRef is the reference point for forming triangles.
|
|
// It's location doesn't change the result (except for rounding error).
|
|
var pRef = Vec2.zero();
|
|
var i;
|
|
var inv3 = 1.0 / 3.0;
|
|
for (var i = 0; i < count; ++i) {
|
|
// Triangle vertices.
|
|
var p1 = pRef;
|
|
var p2 = vs[i];
|
|
var p3 = i + 1 < count ? vs[i + 1] : vs[0];
|
|
var e1 = Vec2.sub(p2, p1);
|
|
var e2 = Vec2.sub(p3, p1);
|
|
var D = Vec2.cross(e1, e2);
|
|
var triangleArea = 0.5 * D;
|
|
area += triangleArea;
|
|
// Area weighted centroid
|
|
c.addMul(triangleArea * inv3, p1);
|
|
c.addMul(triangleArea * inv3, p2);
|
|
c.addMul(triangleArea * inv3, p3);
|
|
}
|
|
c.mul(1.0 / area);
|
|
return c;
|
|
}
|
|
Shape.TYPES[PolygonShape.TYPE] = PolygonShape;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A chain shape is a free form sequence of line segments. The chain has
|
|
* two-sided collision, so you can use inside and outside collision. Therefore,
|
|
* you may use any winding order. Connectivity information is used to create
|
|
* smooth collisions.
|
|
*
|
|
* WARNING: The chain will not collide properly if there are self-intersections.
|
|
*/
|
|
var ChainShape = /** @class */ (function (_super) {
|
|
__extends(ChainShape, _super);
|
|
function ChainShape(vertices, loop) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof ChainShape)) {
|
|
return new ChainShape(vertices, loop);
|
|
}
|
|
_this = _super.call(this) || this;
|
|
_this.m_type = ChainShape.TYPE;
|
|
_this.m_radius = Settings.polygonRadius;
|
|
_this.m_vertices = [];
|
|
_this.m_count = 0;
|
|
_this.m_prevVertex = null;
|
|
_this.m_nextVertex = null;
|
|
_this.m_hasPrevVertex = false;
|
|
_this.m_hasNextVertex = false;
|
|
_this.m_isLoop = !!loop;
|
|
if (vertices && vertices.length) {
|
|
if (loop) {
|
|
_this._createLoop(vertices);
|
|
}
|
|
else {
|
|
_this._createChain(vertices);
|
|
}
|
|
}
|
|
return _this;
|
|
}
|
|
/** @internal */
|
|
ChainShape.prototype._serialize = function () {
|
|
var data = {
|
|
type: this.m_type,
|
|
vertices: this.m_vertices,
|
|
isLoop: this.m_isLoop,
|
|
hasPrevVertex: this.m_hasPrevVertex,
|
|
hasNextVertex: this.m_hasNextVertex,
|
|
prevVertex: null,
|
|
nextVertex: null,
|
|
};
|
|
if (this.m_prevVertex) {
|
|
data.prevVertex = this.m_prevVertex;
|
|
}
|
|
if (this.m_nextVertex) {
|
|
data.nextVertex = this.m_nextVertex;
|
|
}
|
|
return data;
|
|
};
|
|
/** @internal */
|
|
ChainShape._deserialize = function (data, fixture, restore) {
|
|
var vertices = [];
|
|
if (data.vertices) {
|
|
for (var i = 0; i < data.vertices.length; i++) {
|
|
vertices.push(restore(Vec2, data.vertices[i]));
|
|
}
|
|
}
|
|
var shape = new ChainShape(vertices, data.isLoop);
|
|
if (data.prevVertex) {
|
|
shape.setPrevVertex(data.prevVertex);
|
|
}
|
|
if (data.nextVertex) {
|
|
shape.setNextVertex(data.nextVertex);
|
|
}
|
|
return shape;
|
|
};
|
|
// clear() {
|
|
// this.m_vertices.length = 0;
|
|
// this.m_count = 0;
|
|
// }
|
|
/**
|
|
* Create a loop. This automatically adjusts connectivity.
|
|
*
|
|
* @param vertices an array of vertices, these are copied
|
|
* @param count the vertex count
|
|
*/
|
|
ChainShape.prototype._createLoop = function (vertices) {
|
|
for (var i = 1; i < vertices.length; ++i) {
|
|
vertices[i - 1];
|
|
vertices[i];
|
|
}
|
|
this.m_vertices = [];
|
|
this.m_count = vertices.length + 1;
|
|
for (var i = 0; i < vertices.length; ++i) {
|
|
this.m_vertices[i] = Vec2.clone(vertices[i]);
|
|
}
|
|
this.m_vertices[vertices.length] = Vec2.clone(vertices[0]);
|
|
this.m_prevVertex = this.m_vertices[this.m_count - 2];
|
|
this.m_nextVertex = this.m_vertices[1];
|
|
this.m_hasPrevVertex = true;
|
|
this.m_hasNextVertex = true;
|
|
return this;
|
|
};
|
|
/**
|
|
* Create a chain with isolated end vertices.
|
|
*
|
|
* @param vertices an array of vertices, these are copied
|
|
* @param count the vertex count
|
|
*/
|
|
ChainShape.prototype._createChain = function (vertices) {
|
|
for (var i = 1; i < vertices.length; ++i) {
|
|
// If the code crashes here, it means your vertices are too close together.
|
|
vertices[i - 1];
|
|
vertices[i];
|
|
}
|
|
this.m_count = vertices.length;
|
|
for (var i = 0; i < vertices.length; ++i) {
|
|
this.m_vertices[i] = Vec2.clone(vertices[i]);
|
|
}
|
|
this.m_hasPrevVertex = false;
|
|
this.m_hasNextVertex = false;
|
|
this.m_prevVertex = null;
|
|
this.m_nextVertex = null;
|
|
return this;
|
|
};
|
|
ChainShape.prototype._reset = function () {
|
|
if (this.m_isLoop) {
|
|
this._createLoop(this.m_vertices);
|
|
}
|
|
else {
|
|
this._createChain(this.m_vertices);
|
|
}
|
|
};
|
|
/**
|
|
* Establish connectivity to a vertex that precedes the first vertex. Don't call
|
|
* this for loops.
|
|
*/
|
|
ChainShape.prototype.setPrevVertex = function (prevVertex) {
|
|
this.m_prevVertex = prevVertex;
|
|
this.m_hasPrevVertex = true;
|
|
};
|
|
/**
|
|
* Establish connectivity to a vertex that follows the last vertex. Don't call
|
|
* this for loops.
|
|
*/
|
|
ChainShape.prototype.setNextVertex = function (nextVertex) {
|
|
this.m_nextVertex = nextVertex;
|
|
this.m_hasNextVertex = true;
|
|
};
|
|
/**
|
|
* @deprecated Shapes should be treated as immutable.
|
|
*
|
|
* clone the concrete shape.
|
|
*/
|
|
ChainShape.prototype._clone = function () {
|
|
var clone = new ChainShape();
|
|
clone._createChain(this.m_vertices);
|
|
clone.m_type = this.m_type;
|
|
clone.m_radius = this.m_radius;
|
|
clone.m_prevVertex = this.m_prevVertex;
|
|
clone.m_nextVertex = this.m_nextVertex;
|
|
clone.m_hasPrevVertex = this.m_hasPrevVertex;
|
|
clone.m_hasNextVertex = this.m_hasNextVertex;
|
|
return clone;
|
|
};
|
|
/**
|
|
* Get the number of child primitives.
|
|
*/
|
|
ChainShape.prototype.getChildCount = function () {
|
|
// edge count = vertex count - 1
|
|
return this.m_count - 1;
|
|
};
|
|
// Get a child edge.
|
|
ChainShape.prototype.getChildEdge = function (edge, childIndex) {
|
|
edge.m_type = EdgeShape.TYPE;
|
|
edge.m_radius = this.m_radius;
|
|
edge.m_vertex1 = this.m_vertices[childIndex];
|
|
edge.m_vertex2 = this.m_vertices[childIndex + 1];
|
|
if (childIndex > 0) {
|
|
edge.m_vertex0 = this.m_vertices[childIndex - 1];
|
|
edge.m_hasVertex0 = true;
|
|
}
|
|
else {
|
|
edge.m_vertex0 = this.m_prevVertex;
|
|
edge.m_hasVertex0 = this.m_hasPrevVertex;
|
|
}
|
|
if (childIndex < this.m_count - 2) {
|
|
edge.m_vertex3 = this.m_vertices[childIndex + 2];
|
|
edge.m_hasVertex3 = true;
|
|
}
|
|
else {
|
|
edge.m_vertex3 = this.m_nextVertex;
|
|
edge.m_hasVertex3 = this.m_hasNextVertex;
|
|
}
|
|
};
|
|
ChainShape.prototype.getVertex = function (index) {
|
|
if (index < this.m_count) {
|
|
return this.m_vertices[index];
|
|
}
|
|
else {
|
|
return this.m_vertices[0];
|
|
}
|
|
};
|
|
/**
|
|
* Test a point for containment in this shape. This only works for convex
|
|
* shapes.
|
|
*
|
|
* This always return false.
|
|
*
|
|
* @param {Transform} xf The shape world transform.
|
|
* @param p A point in world coordinates.
|
|
*/
|
|
ChainShape.prototype.testPoint = function (xf, p) {
|
|
return false;
|
|
};
|
|
/**
|
|
* Cast a ray against a child shape.
|
|
*
|
|
* @param {RayCastOutput} output The ray-cast results.
|
|
* @param {RayCastInput} input The ray-cast input parameters.
|
|
* @param {Transform} transform The transform to be applied to the shape.
|
|
* @param childIndex The child shape index
|
|
*/
|
|
ChainShape.prototype.rayCast = function (output, input, xf, childIndex) {
|
|
var edgeShape = new EdgeShape(this.getVertex(childIndex), this.getVertex(childIndex + 1));
|
|
return edgeShape.rayCast(output, input, xf, 0);
|
|
};
|
|
/**
|
|
* Given a transform, compute the associated axis aligned bounding box for a
|
|
* child shape.
|
|
*
|
|
* @param {AABB} aabb Returns the axis aligned box.
|
|
* @param {Transform} xf The world transform of the shape.
|
|
* @param childIndex The child shape
|
|
*/
|
|
ChainShape.prototype.computeAABB = function (aabb, xf, childIndex) {
|
|
var v1 = Transform.mulVec2(xf, this.getVertex(childIndex));
|
|
var v2 = Transform.mulVec2(xf, this.getVertex(childIndex + 1));
|
|
aabb.combinePoints(v1, v2);
|
|
};
|
|
/**
|
|
* Compute the mass properties of this shape using its dimensions and density.
|
|
* The inertia tensor is computed about the local origin.
|
|
*
|
|
* Chains have zero mass.
|
|
*
|
|
* @param {MassData} massData Returns the mass data for this shape.
|
|
* @param density The density in kilograms per meter squared.
|
|
*/
|
|
ChainShape.prototype.computeMass = function (massData, density) {
|
|
massData.mass = 0.0;
|
|
massData.center = Vec2.zero();
|
|
massData.I = 0.0;
|
|
};
|
|
ChainShape.prototype.computeDistanceProxy = function (proxy, childIndex) {
|
|
proxy.m_buffer[0] = this.getVertex(childIndex);
|
|
proxy.m_buffer[1] = this.getVertex(childIndex + 1);
|
|
proxy.m_vertices = proxy.m_buffer;
|
|
proxy.m_count = 2;
|
|
proxy.m_radius = this.m_radius;
|
|
};
|
|
ChainShape.TYPE = 'chain';
|
|
return ChainShape;
|
|
}(Shape));
|
|
Shape.TYPES[ChainShape.TYPE] = ChainShape;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
/**
|
|
* A rectangle polygon which extend PolygonShape.
|
|
*/
|
|
var BoxShape = /** @class */ (function (_super) {
|
|
__extends(BoxShape, _super);
|
|
function BoxShape(hx, hy, center, angle) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof BoxShape)) {
|
|
return new BoxShape(hx, hy, center, angle);
|
|
}
|
|
_this = _super.call(this) || this;
|
|
_this._setAsBox(hx, hy, center, angle);
|
|
return _this;
|
|
}
|
|
BoxShape.TYPE = 'polygon';
|
|
return BoxShape;
|
|
}(PolygonShape));
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
Contact.addType(CircleShape.TYPE, CircleShape.TYPE, CircleCircleContact);
|
|
function CircleCircleContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB, indexB) {
|
|
CollideCircles(manifold, fixtureA.getShape(), xfA, fixtureB.getShape(), xfB);
|
|
}
|
|
function CollideCircles(manifold, circleA, xfA, circleB, xfB) {
|
|
manifold.pointCount = 0;
|
|
var pA = Transform.mulVec2(xfA, circleA.m_p);
|
|
var pB = Transform.mulVec2(xfB, circleB.m_p);
|
|
var distSqr = Vec2.distanceSquared(pB, pA);
|
|
var rA = circleA.m_radius;
|
|
var rB = circleB.m_radius;
|
|
var radius = rA + rB;
|
|
if (distSqr > radius * radius) {
|
|
return;
|
|
}
|
|
manifold.type = ManifoldType.e_circles;
|
|
manifold.localPoint.set(circleA.m_p);
|
|
manifold.localNormal.setZero();
|
|
manifold.pointCount = 1;
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
Contact.addType(EdgeShape.TYPE, CircleShape.TYPE, EdgeCircleContact);
|
|
Contact.addType(ChainShape.TYPE, CircleShape.TYPE, ChainCircleContact);
|
|
function EdgeCircleContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB, indexB) {
|
|
var shapeA = fixtureA.getShape();
|
|
var shapeB = fixtureB.getShape();
|
|
CollideEdgeCircle(manifold, shapeA, xfA, shapeB, xfB);
|
|
}
|
|
function ChainCircleContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB, indexB) {
|
|
var chain = fixtureA.getShape();
|
|
var edge = new EdgeShape();
|
|
chain.getChildEdge(edge, indexA);
|
|
var shapeA = edge;
|
|
var shapeB = fixtureB.getShape();
|
|
CollideEdgeCircle(manifold, shapeA, xfA, shapeB, xfB);
|
|
}
|
|
// Compute contact points for edge versus circle.
|
|
// This accounts for edge connectivity.
|
|
function CollideEdgeCircle(manifold, edgeA, xfA, circleB, xfB) {
|
|
manifold.pointCount = 0;
|
|
// Compute circle in frame of edge
|
|
var Q = Transform.mulTVec2(xfA, Transform.mulVec2(xfB, circleB.m_p));
|
|
var A = edgeA.m_vertex1;
|
|
var B = edgeA.m_vertex2;
|
|
var e = Vec2.sub(B, A);
|
|
// Barycentric coordinates
|
|
var u = Vec2.dot(e, Vec2.sub(B, Q));
|
|
var v = Vec2.dot(e, Vec2.sub(Q, A));
|
|
var radius = edgeA.m_radius + circleB.m_radius;
|
|
// Region A
|
|
if (v <= 0.0) {
|
|
var P_1 = Vec2.clone(A);
|
|
var d_1 = Vec2.sub(Q, P_1);
|
|
var dd_1 = Vec2.dot(d_1, d_1);
|
|
if (dd_1 > radius * radius) {
|
|
return;
|
|
}
|
|
// Is there an edge connected to A?
|
|
if (edgeA.m_hasVertex0) {
|
|
var A1 = edgeA.m_vertex0;
|
|
var B1 = A;
|
|
var e1 = Vec2.sub(B1, A1);
|
|
var u1 = Vec2.dot(e1, Vec2.sub(B1, Q));
|
|
// Is the circle in Region AB of the previous edge?
|
|
if (u1 > 0.0) {
|
|
return;
|
|
}
|
|
}
|
|
manifold.type = ManifoldType.e_circles;
|
|
manifold.localNormal.setZero();
|
|
manifold.localPoint.set(P_1);
|
|
manifold.pointCount = 1;
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
return;
|
|
}
|
|
// Region B
|
|
if (u <= 0.0) {
|
|
var P_2 = Vec2.clone(B);
|
|
var d_2 = Vec2.sub(Q, P_2);
|
|
var dd_2 = Vec2.dot(d_2, d_2);
|
|
if (dd_2 > radius * radius) {
|
|
return;
|
|
}
|
|
// Is there an edge connected to B?
|
|
if (edgeA.m_hasVertex3) {
|
|
var B2 = edgeA.m_vertex3;
|
|
var A2 = B;
|
|
var e2 = Vec2.sub(B2, A2);
|
|
var v2 = Vec2.dot(e2, Vec2.sub(Q, A2));
|
|
// Is the circle in Region AB of the next edge?
|
|
if (v2 > 0.0) {
|
|
return;
|
|
}
|
|
}
|
|
manifold.type = ManifoldType.e_circles;
|
|
manifold.localNormal.setZero();
|
|
manifold.localPoint.set(P_2);
|
|
manifold.pointCount = 1;
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 1;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
return;
|
|
}
|
|
// Region AB
|
|
var den = Vec2.dot(e, e);
|
|
var P = Vec2.combine(u / den, A, v / den, B);
|
|
var d = Vec2.sub(Q, P);
|
|
var dd = Vec2.dot(d, d);
|
|
if (dd > radius * radius) {
|
|
return;
|
|
}
|
|
var n = Vec2.neo(-e.y, e.x);
|
|
if (Vec2.dot(n, Vec2.sub(Q, A)) < 0.0) {
|
|
n.set(-n.x, -n.y);
|
|
}
|
|
n.normalize();
|
|
manifold.type = ManifoldType.e_faceA;
|
|
manifold.localNormal = n;
|
|
manifold.localPoint.set(A);
|
|
manifold.pointCount = 1;
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_face;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
Contact.addType(PolygonShape.TYPE, PolygonShape.TYPE, PolygonContact);
|
|
function PolygonContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB, indexB) {
|
|
CollidePolygons(manifold, fixtureA.getShape(), xfA, fixtureB.getShape(), xfB);
|
|
}
|
|
/**
|
|
* Find the max separation between poly1 and poly2 using edge normals from
|
|
* poly1.
|
|
*/
|
|
function findMaxSeparation(poly1, xf1, poly2, xf2, output) {
|
|
var count1 = poly1.m_count;
|
|
var count2 = poly2.m_count;
|
|
var n1s = poly1.m_normals;
|
|
var v1s = poly1.m_vertices;
|
|
var v2s = poly2.m_vertices;
|
|
var xf = Transform.mulTXf(xf2, xf1);
|
|
var bestIndex = 0;
|
|
var maxSeparation = -Infinity;
|
|
for (var i = 0; i < count1; ++i) {
|
|
// Get poly1 normal in frame2.
|
|
var n = Rot.mulVec2(xf.q, n1s[i]);
|
|
var v1 = Transform.mulVec2(xf, v1s[i]);
|
|
// Find deepest point for normal i.
|
|
var si = Infinity;
|
|
for (var j = 0; j < count2; ++j) {
|
|
var sij = Vec2.dot(n, v2s[j]) - Vec2.dot(n, v1);
|
|
if (sij < si) {
|
|
si = sij;
|
|
}
|
|
}
|
|
if (si > maxSeparation) {
|
|
maxSeparation = si;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
// used to keep last FindMaxSeparation call values
|
|
output.maxSeparation = maxSeparation;
|
|
output.bestIndex = bestIndex;
|
|
}
|
|
function findIncidentEdge(c, poly1, xf1, edge1, poly2, xf2) {
|
|
var normals1 = poly1.m_normals;
|
|
var count2 = poly2.m_count;
|
|
var vertices2 = poly2.m_vertices;
|
|
var normals2 = poly2.m_normals;
|
|
// Get the normal of the reference edge in poly2's frame.
|
|
var normal1 = Rot.mulT(xf2.q, Rot.mulVec2(xf1.q, normals1[edge1]));
|
|
// Find the incident edge on poly2.
|
|
var index = 0;
|
|
var minDot = Infinity;
|
|
for (var i = 0; i < count2; ++i) {
|
|
var dot = Vec2.dot(normal1, normals2[i]);
|
|
if (dot < minDot) {
|
|
minDot = dot;
|
|
index = i;
|
|
}
|
|
}
|
|
// Build the clip vertices for the incident edge.
|
|
var i1 = index;
|
|
var i2 = i1 + 1 < count2 ? i1 + 1 : 0;
|
|
c[0].v = Transform.mulVec2(xf2, vertices2[i1]);
|
|
c[0].id.cf.indexA = edge1;
|
|
c[0].id.cf.indexB = i1;
|
|
c[0].id.cf.typeA = ContactFeatureType.e_face;
|
|
c[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
c[1].v = Transform.mulVec2(xf2, vertices2[i2]);
|
|
c[1].id.cf.indexA = edge1;
|
|
c[1].id.cf.indexB = i2;
|
|
c[1].id.cf.typeA = ContactFeatureType.e_face;
|
|
c[1].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
var maxSeparation = {
|
|
maxSeparation: 0,
|
|
bestIndex: 0,
|
|
};
|
|
/**
|
|
*
|
|
* Find edge normal of max separation on A - return if separating axis is found<br>
|
|
* Find edge normal of max separation on B - return if separation axis is found<br>
|
|
* Choose reference edge as min(minA, minB)<br>
|
|
* Find incident edge<br>
|
|
* Clip
|
|
*
|
|
* The normal points from 1 to 2
|
|
*/
|
|
function CollidePolygons(manifold, polyA, xfA, polyB, xfB) {
|
|
manifold.pointCount = 0;
|
|
var totalRadius = polyA.m_radius + polyB.m_radius;
|
|
findMaxSeparation(polyA, xfA, polyB, xfB, maxSeparation);
|
|
var edgeA = maxSeparation.bestIndex;
|
|
var separationA = maxSeparation.maxSeparation;
|
|
if (separationA > totalRadius)
|
|
return;
|
|
findMaxSeparation(polyB, xfB, polyA, xfA, maxSeparation);
|
|
var edgeB = maxSeparation.bestIndex;
|
|
var separationB = maxSeparation.maxSeparation;
|
|
if (separationB > totalRadius)
|
|
return;
|
|
var poly1; // reference polygon
|
|
var poly2; // incident polygon
|
|
var xf1;
|
|
var xf2;
|
|
var edge1; // reference edge
|
|
var flip;
|
|
var k_tol = 0.1 * Settings.linearSlop;
|
|
if (separationB > separationA + k_tol) {
|
|
poly1 = polyB;
|
|
poly2 = polyA;
|
|
xf1 = xfB;
|
|
xf2 = xfA;
|
|
edge1 = edgeB;
|
|
manifold.type = ManifoldType.e_faceB;
|
|
flip = 1;
|
|
}
|
|
else {
|
|
poly1 = polyA;
|
|
poly2 = polyB;
|
|
xf1 = xfA;
|
|
xf2 = xfB;
|
|
edge1 = edgeA;
|
|
manifold.type = ManifoldType.e_faceA;
|
|
flip = 0;
|
|
}
|
|
var incidentEdge = [new ClipVertex(), new ClipVertex()];
|
|
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
|
|
var count1 = poly1.m_count;
|
|
var vertices1 = poly1.m_vertices;
|
|
var iv1 = edge1;
|
|
var iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
|
|
var v11 = vertices1[iv1];
|
|
var v12 = vertices1[iv2];
|
|
var localTangent = Vec2.sub(v12, v11);
|
|
localTangent.normalize();
|
|
var localNormal = Vec2.cross(localTangent, 1.0);
|
|
var planePoint = Vec2.combine(0.5, v11, 0.5, v12);
|
|
var tangent = Rot.mulVec2(xf1.q, localTangent);
|
|
var normal = Vec2.cross(tangent, 1.0);
|
|
v11 = Transform.mulVec2(xf1, v11);
|
|
v12 = Transform.mulVec2(xf1, v12);
|
|
// Face offset.
|
|
var frontOffset = Vec2.dot(normal, v11);
|
|
// Side offsets, extended by polytope skin thickness.
|
|
var sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
|
|
var sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
|
|
// Clip incident edge against extruded edge1 side edges.
|
|
var clipPoints1 = [new ClipVertex(), new ClipVertex()];
|
|
var clipPoints2 = [new ClipVertex(), new ClipVertex()];
|
|
var np;
|
|
// Clip to box side 1
|
|
np = clipSegmentToLine(clipPoints1, incidentEdge, Vec2.neg(tangent), sideOffset1, iv1);
|
|
if (np < 2) {
|
|
return;
|
|
}
|
|
// Clip to negative box side 1
|
|
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
|
|
if (np < 2) {
|
|
return;
|
|
}
|
|
// Now clipPoints2 contains the clipped points.
|
|
manifold.localNormal = localNormal;
|
|
manifold.localPoint = planePoint;
|
|
var pointCount = 0;
|
|
for (var i = 0; i < clipPoints2.length /* maxManifoldPoints */; ++i) {
|
|
var separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
|
|
if (separation <= totalRadius) {
|
|
var cp = manifold.points[pointCount];
|
|
cp.localPoint.set(Transform.mulTVec2(xf2, clipPoints2[i].v));
|
|
cp.id = clipPoints2[i].id;
|
|
if (flip) {
|
|
// Swap features
|
|
var cf = cp.id.cf;
|
|
var indexA = cf.indexA;
|
|
var indexB = cf.indexB;
|
|
var typeA = cf.typeA;
|
|
var typeB = cf.typeB;
|
|
cf.indexA = indexB;
|
|
cf.indexB = indexA;
|
|
cf.typeA = typeB;
|
|
cf.typeB = typeA;
|
|
}
|
|
++pointCount;
|
|
}
|
|
}
|
|
manifold.pointCount = pointCount;
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
Contact.addType(PolygonShape.TYPE, CircleShape.TYPE, PolygonCircleContact);
|
|
function PolygonCircleContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB, indexB) {
|
|
CollidePolygonCircle(manifold, fixtureA.getShape(), xfA, fixtureB.getShape(), xfB);
|
|
}
|
|
function CollidePolygonCircle(manifold, polygonA, xfA, circleB, xfB) {
|
|
manifold.pointCount = 0;
|
|
// Compute circle position in the frame of the polygon.
|
|
var c = Transform.mulVec2(xfB, circleB.m_p);
|
|
var cLocal = Transform.mulTVec2(xfA, c);
|
|
// Find the min separating edge.
|
|
var normalIndex = 0;
|
|
var separation = -Infinity;
|
|
var radius = polygonA.m_radius + circleB.m_radius;
|
|
var vertexCount = polygonA.m_count;
|
|
var vertices = polygonA.m_vertices;
|
|
var normals = polygonA.m_normals;
|
|
for (var i = 0; i < vertexCount; ++i) {
|
|
var s = Vec2.dot(normals[i], Vec2.sub(cLocal, vertices[i]));
|
|
if (s > radius) {
|
|
// Early out.
|
|
return;
|
|
}
|
|
if (s > separation) {
|
|
separation = s;
|
|
normalIndex = i;
|
|
}
|
|
}
|
|
// Vertices that subtend the incident face.
|
|
var vertIndex1 = normalIndex;
|
|
var vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
|
|
var v1 = vertices[vertIndex1];
|
|
var v2 = vertices[vertIndex2];
|
|
// If the center is inside the polygon ...
|
|
if (separation < math.EPSILON) {
|
|
manifold.pointCount = 1;
|
|
manifold.type = ManifoldType.e_faceA;
|
|
manifold.localNormal.set(normals[normalIndex]);
|
|
manifold.localPoint.setCombine(0.5, v1, 0.5, v2);
|
|
manifold.points[0].localPoint = circleB.m_p;
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
return;
|
|
}
|
|
// Compute barycentric coordinates
|
|
var u1 = Vec2.dot(Vec2.sub(cLocal, v1), Vec2.sub(v2, v1));
|
|
var u2 = Vec2.dot(Vec2.sub(cLocal, v2), Vec2.sub(v1, v2));
|
|
if (u1 <= 0.0) {
|
|
if (Vec2.distanceSquared(cLocal, v1) > radius * radius) {
|
|
return;
|
|
}
|
|
manifold.pointCount = 1;
|
|
manifold.type = ManifoldType.e_faceA;
|
|
manifold.localNormal.setCombine(1, cLocal, -1, v1);
|
|
manifold.localNormal.normalize();
|
|
manifold.localPoint = v1;
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
else if (u2 <= 0.0) {
|
|
if (Vec2.distanceSquared(cLocal, v2) > radius * radius) {
|
|
return;
|
|
}
|
|
manifold.pointCount = 1;
|
|
manifold.type = ManifoldType.e_faceA;
|
|
manifold.localNormal.setCombine(1, cLocal, -1, v2);
|
|
manifold.localNormal.normalize();
|
|
manifold.localPoint.set(v2);
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
else {
|
|
var faceCenter = Vec2.mid(v1, v2);
|
|
var separation_1 = Vec2.dot(cLocal, normals[vertIndex1]) - Vec2.dot(faceCenter, normals[vertIndex1]);
|
|
if (separation_1 > radius) {
|
|
return;
|
|
}
|
|
manifold.pointCount = 1;
|
|
manifold.type = ManifoldType.e_faceA;
|
|
manifold.localNormal.set(normals[vertIndex1]);
|
|
manifold.localPoint.set(faceCenter);
|
|
manifold.points[0].localPoint.set(circleB.m_p);
|
|
// manifold.points[0].id.key = 0;
|
|
manifold.points[0].id.cf.indexA = 0;
|
|
manifold.points[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
manifold.points[0].id.cf.indexB = 0;
|
|
manifold.points[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
Contact.addType(EdgeShape.TYPE, PolygonShape.TYPE, EdgePolygonContact);
|
|
Contact.addType(ChainShape.TYPE, PolygonShape.TYPE, ChainPolygonContact);
|
|
function EdgePolygonContact(manifold, xfA, fA, indexA, xfB, fB, indexB) {
|
|
CollideEdgePolygon(manifold, fA.getShape(), xfA, fB.getShape(), xfB);
|
|
}
|
|
function ChainPolygonContact(manifold, xfA, fA, indexA, xfB, fB, indexB) {
|
|
var chain = fA.getShape();
|
|
var edge = new EdgeShape();
|
|
chain.getChildEdge(edge, indexA);
|
|
CollideEdgePolygon(manifold, edge, xfA, fB.getShape(), xfB);
|
|
}
|
|
var EPAxisType;
|
|
(function (EPAxisType) {
|
|
EPAxisType[EPAxisType["e_unknown"] = -1] = "e_unknown";
|
|
EPAxisType[EPAxisType["e_edgeA"] = 1] = "e_edgeA";
|
|
EPAxisType[EPAxisType["e_edgeB"] = 2] = "e_edgeB";
|
|
})(EPAxisType || (EPAxisType = {}));
|
|
// unused?
|
|
var VertexType;
|
|
(function (VertexType) {
|
|
VertexType[VertexType["e_isolated"] = 0] = "e_isolated";
|
|
VertexType[VertexType["e_concave"] = 1] = "e_concave";
|
|
VertexType[VertexType["e_convex"] = 2] = "e_convex";
|
|
})(VertexType || (VertexType = {}));
|
|
/**
|
|
* This structure is used to keep track of the best separating axis.
|
|
*/
|
|
var EPAxis = /** @class */ (function () {
|
|
function EPAxis() {
|
|
}
|
|
return EPAxis;
|
|
}());
|
|
/**
|
|
* This holds polygon B expressed in frame A.
|
|
*/
|
|
var TempPolygon = /** @class */ (function () {
|
|
function TempPolygon() {
|
|
this.vertices = []; // [Settings.maxPolygonVertices]
|
|
this.normals = []; // [Settings.maxPolygonVertices];
|
|
this.count = 0;
|
|
}
|
|
return TempPolygon;
|
|
}());
|
|
/**
|
|
* Reference face used for clipping
|
|
*/
|
|
var ReferenceFace = /** @class */ (function () {
|
|
function ReferenceFace() {
|
|
this.normal = Vec2.zero();
|
|
this.sideNormal1 = Vec2.zero();
|
|
this.sideNormal2 = Vec2.zero();
|
|
}
|
|
return ReferenceFace;
|
|
}());
|
|
// reused
|
|
var edgeAxis = new EPAxis();
|
|
var polygonAxis = new EPAxis();
|
|
var polygonBA = new TempPolygon();
|
|
var rf = new ReferenceFace();
|
|
/**
|
|
* This function collides and edge and a polygon, taking into account edge
|
|
* adjacency.
|
|
*/
|
|
function CollideEdgePolygon(manifold, edgeA, xfA, polygonB, xfB) {
|
|
// Algorithm:
|
|
// 1. Classify v1 and v2
|
|
// 2. Classify polygon centroid as front or back
|
|
// 3. Flip normal if necessary
|
|
// 4. Initialize normal range to [-pi, pi] about face normal
|
|
// 5. Adjust normal range according to adjacent edges
|
|
// 6. Visit each separating axes, only accept axes within the range
|
|
// 7. Return if _any_ axis indicates separation
|
|
// 8. Clip
|
|
// let m_type1: VertexType;
|
|
// let m_type2: VertexType;
|
|
var xf = Transform.mulTXf(xfA, xfB);
|
|
var centroidB = Transform.mulVec2(xf, polygonB.m_centroid);
|
|
var v0 = edgeA.m_vertex0;
|
|
var v1 = edgeA.m_vertex1;
|
|
var v2 = edgeA.m_vertex2;
|
|
var v3 = edgeA.m_vertex3;
|
|
var hasVertex0 = edgeA.m_hasVertex0;
|
|
var hasVertex3 = edgeA.m_hasVertex3;
|
|
var edge1 = Vec2.sub(v2, v1);
|
|
edge1.normalize();
|
|
var normal1 = Vec2.neo(edge1.y, -edge1.x);
|
|
var offset1 = Vec2.dot(normal1, Vec2.sub(centroidB, v1));
|
|
var offset0 = 0.0;
|
|
var offset2 = 0.0;
|
|
var convex1 = false;
|
|
var convex2 = false;
|
|
var normal0;
|
|
var normal2;
|
|
// Is there a preceding edge?
|
|
if (hasVertex0) {
|
|
var edge0 = Vec2.sub(v1, v0);
|
|
edge0.normalize();
|
|
normal0 = Vec2.neo(edge0.y, -edge0.x);
|
|
convex1 = Vec2.cross(edge0, edge1) >= 0.0;
|
|
offset0 = Vec2.dot(normal0, centroidB) - Vec2.dot(normal0, v0);
|
|
}
|
|
// Is there a following edge?
|
|
if (hasVertex3) {
|
|
var edge2 = Vec2.sub(v3, v2);
|
|
edge2.normalize();
|
|
normal2 = Vec2.neo(edge2.y, -edge2.x);
|
|
convex2 = Vec2.cross(edge1, edge2) > 0.0;
|
|
offset2 = Vec2.dot(normal2, centroidB) - Vec2.dot(normal2, v2);
|
|
}
|
|
var front;
|
|
var normal = Vec2.zero();
|
|
var lowerLimit = Vec2.zero();
|
|
var upperLimit = Vec2.zero();
|
|
// Determine front or back collision. Determine collision normal limits.
|
|
if (hasVertex0 && hasVertex3) {
|
|
if (convex1 && convex2) {
|
|
front = offset0 >= 0.0 || offset1 >= 0.0 || offset2 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal0);
|
|
upperLimit.set(normal2);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
}
|
|
else if (convex1) {
|
|
front = offset0 >= 0.0 || (offset1 >= 0.0 && offset2 >= 0.0);
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal0);
|
|
upperLimit.set(normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal2);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
}
|
|
else if (convex2) {
|
|
front = offset2 >= 0.0 || (offset0 >= 0.0 && offset1 >= 0.0);
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.set(normal2);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.setMul(-1, normal0);
|
|
}
|
|
}
|
|
else {
|
|
front = offset0 >= 0.0 && offset1 >= 0.0 && offset2 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.set(normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal2);
|
|
upperLimit.setMul(-1, normal0);
|
|
}
|
|
}
|
|
}
|
|
else if (hasVertex0) {
|
|
if (convex1) {
|
|
front = offset0 >= 0.0 || offset1 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal0);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
}
|
|
else {
|
|
front = offset0 >= 0.0 && offset1 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.setMul(-1, normal0);
|
|
}
|
|
}
|
|
}
|
|
else if (hasVertex3) {
|
|
if (convex2) {
|
|
front = offset1 >= 0.0 || offset2 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.set(normal2);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.set(normal1);
|
|
}
|
|
}
|
|
else {
|
|
front = offset1 >= 0.0 && offset2 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.set(normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.setMul(-1, normal2);
|
|
upperLimit.set(normal1);
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
front = offset1 >= 0.0;
|
|
if (front) {
|
|
normal.set(normal1);
|
|
lowerLimit.setMul(-1, normal1);
|
|
upperLimit.setMul(-1, normal1);
|
|
}
|
|
else {
|
|
normal.setMul(-1, normal1);
|
|
lowerLimit.set(normal1);
|
|
upperLimit.set(normal1);
|
|
}
|
|
}
|
|
// Get polygonB in frameA
|
|
polygonBA.count = polygonB.m_count;
|
|
for (var i = 0; i < polygonB.m_count; ++i) {
|
|
polygonBA.vertices[i] = Transform.mulVec2(xf, polygonB.m_vertices[i]);
|
|
polygonBA.normals[i] = Rot.mulVec2(xf.q, polygonB.m_normals[i]);
|
|
}
|
|
var radius = 2.0 * Settings.polygonRadius;
|
|
manifold.pointCount = 0;
|
|
{ // ComputeEdgeSeparation
|
|
edgeAxis.type = EPAxisType.e_edgeA;
|
|
edgeAxis.index = front ? 0 : 1;
|
|
edgeAxis.separation = Infinity;
|
|
for (var i = 0; i < polygonBA.count; ++i) {
|
|
var s = Vec2.dot(normal, Vec2.sub(polygonBA.vertices[i], v1));
|
|
if (s < edgeAxis.separation) {
|
|
edgeAxis.separation = s;
|
|
}
|
|
}
|
|
}
|
|
// If no valid normal can be found than this edge should not collide.
|
|
// @ts-ignore
|
|
if (edgeAxis.type == EPAxisType.e_unknown) {
|
|
return;
|
|
}
|
|
if (edgeAxis.separation > radius) {
|
|
return;
|
|
}
|
|
{ // ComputePolygonSeparation
|
|
polygonAxis.type = EPAxisType.e_unknown;
|
|
polygonAxis.index = -1;
|
|
polygonAxis.separation = -Infinity;
|
|
var perp = Vec2.neo(-normal.y, normal.x);
|
|
for (var i = 0; i < polygonBA.count; ++i) {
|
|
var n = Vec2.neg(polygonBA.normals[i]);
|
|
var s1 = Vec2.dot(n, Vec2.sub(polygonBA.vertices[i], v1));
|
|
var s2 = Vec2.dot(n, Vec2.sub(polygonBA.vertices[i], v2));
|
|
var s = math.min(s1, s2);
|
|
if (s > radius) {
|
|
// No collision
|
|
polygonAxis.type = EPAxisType.e_edgeB;
|
|
polygonAxis.index = i;
|
|
polygonAxis.separation = s;
|
|
break;
|
|
}
|
|
// Adjacency
|
|
if (Vec2.dot(n, perp) >= 0.0) {
|
|
if (Vec2.dot(Vec2.sub(n, upperLimit), normal) < -Settings.angularSlop) {
|
|
continue;
|
|
}
|
|
}
|
|
else {
|
|
if (Vec2.dot(Vec2.sub(n, lowerLimit), normal) < -Settings.angularSlop) {
|
|
continue;
|
|
}
|
|
}
|
|
if (s > polygonAxis.separation) {
|
|
polygonAxis.type = EPAxisType.e_edgeB;
|
|
polygonAxis.index = i;
|
|
polygonAxis.separation = s;
|
|
}
|
|
}
|
|
}
|
|
if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > radius) {
|
|
return;
|
|
}
|
|
// Use hysteresis for jitter reduction.
|
|
var k_relativeTol = 0.98;
|
|
var k_absoluteTol = 0.001;
|
|
var primaryAxis;
|
|
if (polygonAxis.type == EPAxisType.e_unknown) {
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) {
|
|
primaryAxis = polygonAxis;
|
|
}
|
|
else {
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
var ie = [new ClipVertex(), new ClipVertex()];
|
|
if (primaryAxis.type == EPAxisType.e_edgeA) {
|
|
manifold.type = ManifoldType.e_faceA;
|
|
// Search for the polygon normal that is most anti-parallel to the edge
|
|
// normal.
|
|
var bestIndex = 0;
|
|
var bestValue = Vec2.dot(normal, polygonBA.normals[0]);
|
|
for (var i = 1; i < polygonBA.count; ++i) {
|
|
var value = Vec2.dot(normal, polygonBA.normals[i]);
|
|
if (value < bestValue) {
|
|
bestValue = value;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
var i1 = bestIndex;
|
|
var i2 = i1 + 1 < polygonBA.count ? i1 + 1 : 0;
|
|
ie[0].v = polygonBA.vertices[i1];
|
|
ie[0].id.cf.indexA = 0;
|
|
ie[0].id.cf.indexB = i1;
|
|
ie[0].id.cf.typeA = ContactFeatureType.e_face;
|
|
ie[0].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
ie[1].v = polygonBA.vertices[i2];
|
|
ie[1].id.cf.indexA = 0;
|
|
ie[1].id.cf.indexB = i2;
|
|
ie[1].id.cf.typeA = ContactFeatureType.e_face;
|
|
ie[1].id.cf.typeB = ContactFeatureType.e_vertex;
|
|
if (front) {
|
|
rf.i1 = 0;
|
|
rf.i2 = 1;
|
|
rf.v1 = v1;
|
|
rf.v2 = v2;
|
|
rf.normal.set(normal1);
|
|
}
|
|
else {
|
|
rf.i1 = 1;
|
|
rf.i2 = 0;
|
|
rf.v1 = v2;
|
|
rf.v2 = v1;
|
|
rf.normal.setMul(-1, normal1);
|
|
}
|
|
}
|
|
else {
|
|
manifold.type = ManifoldType.e_faceB;
|
|
ie[0].v = v1;
|
|
ie[0].id.cf.indexA = 0;
|
|
ie[0].id.cf.indexB = primaryAxis.index;
|
|
ie[0].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
ie[0].id.cf.typeB = ContactFeatureType.e_face;
|
|
ie[1].v = v2;
|
|
ie[1].id.cf.indexA = 0;
|
|
ie[1].id.cf.indexB = primaryAxis.index;
|
|
ie[1].id.cf.typeA = ContactFeatureType.e_vertex;
|
|
ie[1].id.cf.typeB = ContactFeatureType.e_face;
|
|
rf.i1 = primaryAxis.index;
|
|
rf.i2 = rf.i1 + 1 < polygonBA.count ? rf.i1 + 1 : 0;
|
|
rf.v1 = polygonBA.vertices[rf.i1];
|
|
rf.v2 = polygonBA.vertices[rf.i2];
|
|
rf.normal.set(polygonBA.normals[rf.i1]);
|
|
}
|
|
rf.sideNormal1.set(rf.normal.y, -rf.normal.x);
|
|
rf.sideNormal2.setMul(-1, rf.sideNormal1);
|
|
rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1);
|
|
rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2);
|
|
// Clip incident edge against extruded edge1 side edges.
|
|
var clipPoints1 = [new ClipVertex(), new ClipVertex()];
|
|
var clipPoints2 = [new ClipVertex(), new ClipVertex()];
|
|
var np;
|
|
// Clip to box side 1
|
|
np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
|
|
if (np < Settings.maxManifoldPoints) {
|
|
return;
|
|
}
|
|
// Clip to negative box side 1
|
|
np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
|
|
if (np < Settings.maxManifoldPoints) {
|
|
return;
|
|
}
|
|
// Now clipPoints2 contains the clipped points.
|
|
if (primaryAxis.type == EPAxisType.e_edgeA) {
|
|
manifold.localNormal = Vec2.clone(rf.normal);
|
|
manifold.localPoint = Vec2.clone(rf.v1);
|
|
}
|
|
else {
|
|
manifold.localNormal = Vec2.clone(polygonB.m_normals[rf.i1]);
|
|
manifold.localPoint = Vec2.clone(polygonB.m_vertices[rf.i1]);
|
|
}
|
|
var pointCount = 0;
|
|
for (var i = 0; i < Settings.maxManifoldPoints; ++i) {
|
|
var separation = Vec2.dot(rf.normal, Vec2.sub(clipPoints2[i].v, rf.v1));
|
|
if (separation <= radius) {
|
|
var cp = manifold.points[pointCount]; // ManifoldPoint
|
|
if (primaryAxis.type == EPAxisType.e_edgeA) {
|
|
cp.localPoint = Transform.mulT(xf, clipPoints2[i].v);
|
|
cp.id = clipPoints2[i].id;
|
|
}
|
|
else {
|
|
cp.localPoint = clipPoints2[i].v;
|
|
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
|
|
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
|
|
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
|
|
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
|
|
}
|
|
++pointCount;
|
|
}
|
|
}
|
|
manifold.pointCount = pointCount;
|
|
}
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$a = {
|
|
frequencyHz: 0.0,
|
|
dampingRatio: 0.0
|
|
};
|
|
/**
|
|
* A distance joint constrains two points on two bodies to remain at a fixed
|
|
* distance from each other. You can view this as a massless, rigid rod.
|
|
*
|
|
* @param anchorA Anchor A in global coordination.
|
|
* @param anchorB Anchor B in global coordination.
|
|
*/
|
|
var DistanceJoint = /** @class */ (function (_super) {
|
|
__extends(DistanceJoint, _super);
|
|
function DistanceJoint(def, bodyA, bodyB, anchorA, anchorB) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof DistanceJoint)) {
|
|
return new DistanceJoint(def, bodyA, bodyB, anchorA, anchorB);
|
|
}
|
|
// order of constructor arguments is changed in v0.2
|
|
if (bodyB && anchorA && ('m_type' in anchorA) && ('x' in bodyB) && ('y' in bodyB)) {
|
|
var temp = bodyB;
|
|
bodyB = anchorA;
|
|
anchorA = temp;
|
|
}
|
|
def = options(def, DEFAULTS$a);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = DistanceJoint.TYPE;
|
|
// Solver shared
|
|
_this.m_localAnchorA = Vec2.clone(anchorA ? bodyA.getLocalPoint(anchorA) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchorB ? bodyB.getLocalPoint(anchorB) : def.localAnchorB || Vec2.zero());
|
|
_this.m_length = math.isFinite(def.length) ? def.length :
|
|
Vec2.distance(bodyA.getWorldPoint(_this.m_localAnchorA), bodyB.getWorldPoint(_this.m_localAnchorB));
|
|
_this.m_frequencyHz = def.frequencyHz;
|
|
_this.m_dampingRatio = def.dampingRatio;
|
|
_this.m_impulse = 0.0;
|
|
_this.m_gamma = 0.0;
|
|
_this.m_bias = 0.0;
|
|
// Solver temp
|
|
_this.m_u; // Vec2
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA;
|
|
_this.m_invMassB;
|
|
_this.m_invIA;
|
|
_this.m_invIB;
|
|
_this.m_mass;
|
|
return _this;
|
|
// 1-D constrained system
|
|
// m (v2 - v1) = lambda
|
|
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
|
|
// x2 = x1 + h * v2
|
|
// 1-D mass-damper-spring system
|
|
// m (v2 - v1) + h * d * v2 + h * k *
|
|
// C = norm(p2 - p1) - L
|
|
// u = (p2 - p1) / norm(p2 - p1)
|
|
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
|
|
// J = [-u -cross(r1, u) u cross(r2, u)]
|
|
// K = J * invM * JT
|
|
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
|
|
}
|
|
/** @internal */
|
|
DistanceJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
frequencyHz: this.m_frequencyHz,
|
|
dampingRatio: this.m_dampingRatio,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
length: this.m_length,
|
|
impulse: this.m_impulse,
|
|
gamma: this.m_gamma,
|
|
bias: this.m_bias,
|
|
};
|
|
};
|
|
/** @internal */
|
|
DistanceJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new DistanceJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
DistanceJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
if (def.length > 0) {
|
|
this.m_length = +def.length;
|
|
}
|
|
else if (def.length < 0) ;
|
|
else if (def.anchorA || def.anchorA || def.anchorA || def.anchorA) {
|
|
this.m_length = Vec2.distance(this.m_bodyA.getWorldPoint(this.m_localAnchorA), this.m_bodyB.getWorldPoint(this.m_localAnchorB));
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
DistanceJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
DistanceJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* Set the natural length. Manipulating the length can lead to non-physical
|
|
* behavior when the frequency is zero.
|
|
*/
|
|
DistanceJoint.prototype.setLength = function (length) {
|
|
this.m_length = length;
|
|
};
|
|
/**
|
|
* Get the natural length.
|
|
*/
|
|
DistanceJoint.prototype.getLength = function () {
|
|
return this.m_length;
|
|
};
|
|
DistanceJoint.prototype.setFrequency = function (hz) {
|
|
this.m_frequencyHz = hz;
|
|
};
|
|
DistanceJoint.prototype.getFrequency = function () {
|
|
return this.m_frequencyHz;
|
|
};
|
|
DistanceJoint.prototype.setDampingRatio = function (ratio) {
|
|
this.m_dampingRatio = ratio;
|
|
};
|
|
DistanceJoint.prototype.getDampingRatio = function () {
|
|
return this.m_dampingRatio;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
DistanceJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
DistanceJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
DistanceJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(this.m_impulse, this.m_u).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
DistanceJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return 0.0;
|
|
};
|
|
DistanceJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
this.m_u = Vec2.sub(Vec2.add(cB, this.m_rB), Vec2.add(cA, this.m_rA));
|
|
// Handle singularity.
|
|
var length = this.m_u.length();
|
|
if (length > Settings.linearSlop) {
|
|
this.m_u.mul(1.0 / length);
|
|
}
|
|
else {
|
|
this.m_u.set(0.0, 0.0);
|
|
}
|
|
var crAu = Vec2.cross(this.m_rA, this.m_u);
|
|
var crBu = Vec2.cross(this.m_rB, this.m_u);
|
|
var invMass = this.m_invMassA + this.m_invIA * crAu * crAu + this.m_invMassB
|
|
+ this.m_invIB * crBu * crBu;
|
|
// Compute the effective mass matrix.
|
|
this.m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
|
|
if (this.m_frequencyHz > 0.0) {
|
|
var C = length - this.m_length;
|
|
// Frequency
|
|
var omega = 2.0 * math.PI * this.m_frequencyHz;
|
|
// Damping coefficient
|
|
var d = 2.0 * this.m_mass * this.m_dampingRatio * omega;
|
|
// Spring stiffness
|
|
var k = this.m_mass * omega * omega;
|
|
// magic formulas
|
|
var h = step.dt;
|
|
this.m_gamma = h * (d + h * k);
|
|
this.m_gamma = this.m_gamma != 0.0 ? 1.0 / this.m_gamma : 0.0;
|
|
this.m_bias = C * h * k * this.m_gamma;
|
|
invMass += this.m_gamma;
|
|
this.m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
|
|
}
|
|
else {
|
|
this.m_gamma = 0.0;
|
|
this.m_bias = 0.0;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Scale the impulse to support a variable time step.
|
|
this.m_impulse *= step.dtRatio;
|
|
var P = Vec2.mul(this.m_impulse, this.m_u);
|
|
vA.subMul(this.m_invMassA, P);
|
|
wA -= this.m_invIA * Vec2.cross(this.m_rA, P);
|
|
vB.addMul(this.m_invMassB, P);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, P);
|
|
}
|
|
else {
|
|
this.m_impulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
DistanceJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
// Cdot = dot(u, v + cross(w, r))
|
|
var vpA = Vec2.add(vA, Vec2.cross(wA, this.m_rA));
|
|
var vpB = Vec2.add(vB, Vec2.cross(wB, this.m_rB));
|
|
var Cdot = Vec2.dot(this.m_u, vpB) - Vec2.dot(this.m_u, vpA);
|
|
var impulse = -this.m_mass
|
|
* (Cdot + this.m_bias + this.m_gamma * this.m_impulse);
|
|
this.m_impulse += impulse;
|
|
var P = Vec2.mul(impulse, this.m_u);
|
|
vA.subMul(this.m_invMassA, P);
|
|
wA -= this.m_invIA * Vec2.cross(this.m_rA, P);
|
|
vB.addMul(this.m_invMassB, P);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, P);
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
DistanceJoint.prototype.solvePositionConstraints = function (step) {
|
|
if (this.m_frequencyHz > 0.0) {
|
|
// There is no position correction for soft distance constraints.
|
|
return true;
|
|
}
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var rA = Rot.mulSub(qA, this.m_localAnchorA, this.m_localCenterA);
|
|
var rB = Rot.mulSub(qB, this.m_localAnchorB, this.m_localCenterB);
|
|
var u = Vec2.sub(Vec2.add(cB, rB), Vec2.add(cA, rA));
|
|
var length = u.normalize();
|
|
var C = length - this.m_length;
|
|
C = math
|
|
.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
|
|
var impulse = -this.m_mass * C;
|
|
var P = Vec2.mul(impulse, u);
|
|
cA.subMul(this.m_invMassA, P);
|
|
aA -= this.m_invIA * Vec2.cross(rA, P);
|
|
cB.addMul(this.m_invMassB, P);
|
|
aB += this.m_invIB * Vec2.cross(rB, P);
|
|
this.m_bodyA.c_position.c.set(cA);
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c.set(cB);
|
|
this.m_bodyB.c_position.a = aB;
|
|
return math.abs(C) < Settings.linearSlop;
|
|
};
|
|
DistanceJoint.TYPE = 'distance-joint';
|
|
return DistanceJoint;
|
|
}(Joint));
|
|
Joint.TYPES[DistanceJoint.TYPE] = DistanceJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$9 = {
|
|
maxForce: 0.0,
|
|
maxTorque: 0.0,
|
|
};
|
|
/**
|
|
* Friction joint. This is used for top-down friction. It provides 2D
|
|
* translational friction and angular friction.
|
|
*
|
|
* @param anchor Anchor in global coordination.
|
|
*/
|
|
var FrictionJoint = /** @class */ (function (_super) {
|
|
__extends(FrictionJoint, _super);
|
|
function FrictionJoint(def, bodyA, bodyB, anchor) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof FrictionJoint)) {
|
|
return new FrictionJoint(def, bodyA, bodyB, anchor);
|
|
}
|
|
def = options(def, DEFAULTS$9);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = FrictionJoint.TYPE;
|
|
_this.m_localAnchorA = Vec2.clone(anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.zero());
|
|
// Solver shared
|
|
_this.m_linearImpulse = Vec2.zero();
|
|
_this.m_angularImpulse = 0.0;
|
|
_this.m_maxForce = def.maxForce;
|
|
_this.m_maxTorque = def.maxTorque;
|
|
// Solver temp
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_linearMass; // Mat22
|
|
_this.m_angularMass; // float
|
|
return _this;
|
|
// Point-to-point constraint
|
|
// Cdot = v2 - v1
|
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
|
// J = [-I -r1_skew I r2_skew ]
|
|
// Identity used:
|
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
|
// Angle constraint
|
|
// Cdot = w2 - w1
|
|
// J = [0 0 -1 0 0 1]
|
|
// K = invI1 + invI2
|
|
}
|
|
/** @internal */
|
|
FrictionJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
maxForce: this.m_maxForce,
|
|
maxTorque: this.m_maxTorque,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
};
|
|
};
|
|
/** @internal */
|
|
FrictionJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new FrictionJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
FrictionJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
FrictionJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
FrictionJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* Set the maximum friction force in N.
|
|
*/
|
|
FrictionJoint.prototype.setMaxForce = function (force) {
|
|
this.m_maxForce = force;
|
|
};
|
|
/**
|
|
* Get the maximum friction force in N.
|
|
*/
|
|
FrictionJoint.prototype.getMaxForce = function () {
|
|
return this.m_maxForce;
|
|
};
|
|
/**
|
|
* Set the maximum friction torque in N*m.
|
|
*/
|
|
FrictionJoint.prototype.setMaxTorque = function (torque) {
|
|
this.m_maxTorque = torque;
|
|
};
|
|
/**
|
|
* Get the maximum friction torque in N*m.
|
|
*/
|
|
FrictionJoint.prototype.getMaxTorque = function () {
|
|
return this.m_maxTorque;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
FrictionJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
FrictionJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
FrictionJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(inv_dt, this.m_linearImpulse);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
FrictionJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_angularImpulse;
|
|
};
|
|
FrictionJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA), qB = Rot.neo(aB);
|
|
// Compute the effective mass matrix.
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// J = [-I -r1_skew I r2_skew]
|
|
// [ 0 -1 0 1]
|
|
// r_skew = [-ry; rx]
|
|
// Matlab
|
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
|
var mA = this.m_invMassA, mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA, iB = this.m_invIB; // float
|
|
var K = new Mat22();
|
|
K.ex.x = mA + mB + iA * this.m_rA.y * this.m_rA.y + iB * this.m_rB.y
|
|
* this.m_rB.y;
|
|
K.ex.y = -iA * this.m_rA.x * this.m_rA.y - iB * this.m_rB.x * this.m_rB.y;
|
|
K.ey.x = K.ex.y;
|
|
K.ey.y = mA + mB + iA * this.m_rA.x * this.m_rA.x + iB * this.m_rB.x
|
|
* this.m_rB.x;
|
|
this.m_linearMass = K.getInverse();
|
|
this.m_angularMass = iA + iB;
|
|
if (this.m_angularMass > 0.0) {
|
|
this.m_angularMass = 1.0 / this.m_angularMass;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Scale impulses to support a variable time step.
|
|
this.m_linearImpulse.mul(step.dtRatio);
|
|
this.m_angularImpulse *= step.dtRatio;
|
|
var P = Vec2.neo(this.m_linearImpulse.x, this.m_linearImpulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + this.m_angularImpulse);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + this.m_angularImpulse);
|
|
}
|
|
else {
|
|
this.m_linearImpulse.setZero();
|
|
this.m_angularImpulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
FrictionJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var mA = this.m_invMassA, mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA, iB = this.m_invIB; // float
|
|
var h = step.dt; // float
|
|
// Solve angular friction
|
|
{
|
|
var Cdot = wB - wA; // float
|
|
var impulse = -this.m_angularMass * Cdot; // float
|
|
var oldImpulse = this.m_angularImpulse; // float
|
|
var maxImpulse = h * this.m_maxTorque; // float
|
|
this.m_angularImpulse = math.clamp(this.m_angularImpulse + impulse, -maxImpulse, maxImpulse);
|
|
impulse = this.m_angularImpulse - oldImpulse;
|
|
wA -= iA * impulse;
|
|
wB += iB * impulse;
|
|
}
|
|
// Solve linear friction
|
|
{
|
|
var Cdot = Vec2.sub(Vec2.add(vB, Vec2.cross(wB, this.m_rB)), Vec2.add(vA, Vec2.cross(wA, this.m_rA))); // Vec2
|
|
var impulse = Vec2.neg(Mat22.mulVec2(this.m_linearMass, Cdot)); // Vec2
|
|
var oldImpulse = this.m_linearImpulse; // Vec2
|
|
this.m_linearImpulse.add(impulse);
|
|
var maxImpulse = h * this.m_maxForce; // float
|
|
if (this.m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) {
|
|
this.m_linearImpulse.normalize();
|
|
this.m_linearImpulse.mul(maxImpulse);
|
|
}
|
|
impulse = Vec2.sub(this.m_linearImpulse, oldImpulse);
|
|
vA.subMul(mA, impulse);
|
|
wA -= iA * Vec2.cross(this.m_rA, impulse);
|
|
vB.addMul(mB, impulse);
|
|
wB += iB * Vec2.cross(this.m_rB, impulse);
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
FrictionJoint.prototype.solvePositionConstraints = function (step) {
|
|
return true;
|
|
};
|
|
FrictionJoint.TYPE = 'friction-joint';
|
|
return FrictionJoint;
|
|
}(Joint));
|
|
Joint.TYPES[FrictionJoint.TYPE] = FrictionJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var inactiveLimit$2 = 0;
|
|
var atLowerLimit$1 = 1;
|
|
var atUpperLimit$2 = 2;
|
|
var equalLimits$1 = 3;
|
|
var DEFAULTS$8 = {
|
|
lowerAngle: 0.0,
|
|
upperAngle: 0.0,
|
|
maxMotorTorque: 0.0,
|
|
motorSpeed: 0.0,
|
|
enableLimit: false,
|
|
enableMotor: false
|
|
};
|
|
/**
|
|
* A revolute joint constrains two bodies to share a common point while they are
|
|
* free to rotate about the point. The relative rotation about the shared point
|
|
* is the joint angle. You can limit the relative rotation with a joint limit
|
|
* that specifies a lower and upper angle. You can use a motor to drive the
|
|
* relative rotation about the shared point. A maximum motor torque is provided
|
|
* so that infinite forces are not generated.
|
|
*/
|
|
var RevoluteJoint = /** @class */ (function (_super) {
|
|
__extends(RevoluteJoint, _super);
|
|
function RevoluteJoint(def, bodyA, bodyB, anchor) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof RevoluteJoint)) {
|
|
return new RevoluteJoint(def, bodyA, bodyB, anchor);
|
|
}
|
|
def = options(def, DEFAULTS$8);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = RevoluteJoint.TYPE;
|
|
_this.m_localAnchorA = Vec2.clone(anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.zero());
|
|
_this.m_referenceAngle = math.isFinite(def.referenceAngle) ? def.referenceAngle : bodyB.getAngle() - bodyA.getAngle();
|
|
_this.m_impulse = new Vec3();
|
|
_this.m_motorImpulse = 0.0;
|
|
_this.m_lowerAngle = def.lowerAngle;
|
|
_this.m_upperAngle = def.upperAngle;
|
|
_this.m_maxMotorTorque = def.maxMotorTorque;
|
|
_this.m_motorSpeed = def.motorSpeed;
|
|
_this.m_enableLimit = def.enableLimit;
|
|
_this.m_enableMotor = def.enableMotor;
|
|
// Solver temp
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
// effective mass for point-to-point constraint.
|
|
_this.m_mass = new Mat33();
|
|
// effective mass for motor/limit angular constraint.
|
|
_this.m_motorMass; // float
|
|
_this.m_limitState = inactiveLimit$2;
|
|
return _this;
|
|
// Point-to-point constraint
|
|
// C = p2 - p1
|
|
// Cdot = v2 - v1
|
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
|
// J = [-I -r1_skew I r2_skew ]
|
|
// Identity used:
|
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
|
// Motor constraint
|
|
// Cdot = w2 - w1
|
|
// J = [0 0 -1 0 0 1]
|
|
// K = invI1 + invI2
|
|
}
|
|
/** @internal */
|
|
RevoluteJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
lowerAngle: this.m_lowerAngle,
|
|
upperAngle: this.m_upperAngle,
|
|
maxMotorTorque: this.m_maxMotorTorque,
|
|
motorSpeed: this.m_motorSpeed,
|
|
enableLimit: this.m_enableLimit,
|
|
enableMotor: this.m_enableMotor,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
referenceAngle: this.m_referenceAngle,
|
|
};
|
|
};
|
|
/** @internal */
|
|
RevoluteJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new RevoluteJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
RevoluteJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
RevoluteJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
RevoluteJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* Get the reference angle.
|
|
*/
|
|
RevoluteJoint.prototype.getReferenceAngle = function () {
|
|
return this.m_referenceAngle;
|
|
};
|
|
/**
|
|
* Get the current joint angle in radians.
|
|
*/
|
|
RevoluteJoint.prototype.getJointAngle = function () {
|
|
var bA = this.m_bodyA;
|
|
var bB = this.m_bodyB;
|
|
return bB.m_sweep.a - bA.m_sweep.a - this.m_referenceAngle;
|
|
};
|
|
/**
|
|
* Get the current joint angle speed in radians per second.
|
|
*/
|
|
RevoluteJoint.prototype.getJointSpeed = function () {
|
|
var bA = this.m_bodyA;
|
|
var bB = this.m_bodyB;
|
|
return bB.m_angularVelocity - bA.m_angularVelocity;
|
|
};
|
|
/**
|
|
* Is the joint motor enabled?
|
|
*/
|
|
RevoluteJoint.prototype.isMotorEnabled = function () {
|
|
return this.m_enableMotor;
|
|
};
|
|
/**
|
|
* Enable/disable the joint motor.
|
|
*/
|
|
RevoluteJoint.prototype.enableMotor = function (flag) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_enableMotor = flag;
|
|
};
|
|
/**
|
|
* Get the current motor torque given the inverse time step. Unit is N*m.
|
|
*/
|
|
RevoluteJoint.prototype.getMotorTorque = function (inv_dt) {
|
|
return inv_dt * this.m_motorImpulse;
|
|
};
|
|
/**
|
|
* Set the motor speed in radians per second.
|
|
*/
|
|
RevoluteJoint.prototype.setMotorSpeed = function (speed) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_motorSpeed = speed;
|
|
};
|
|
/**
|
|
* Get the motor speed in radians per second.
|
|
*/
|
|
RevoluteJoint.prototype.getMotorSpeed = function () {
|
|
return this.m_motorSpeed;
|
|
};
|
|
/**
|
|
* Set the maximum motor torque, usually in N-m.
|
|
*/
|
|
RevoluteJoint.prototype.setMaxMotorTorque = function (torque) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_maxMotorTorque = torque;
|
|
};
|
|
RevoluteJoint.prototype.getMaxMotorTorque = function () {
|
|
return this.m_maxMotorTorque;
|
|
};
|
|
/**
|
|
* Is the joint limit enabled?
|
|
*/
|
|
RevoluteJoint.prototype.isLimitEnabled = function () {
|
|
return this.m_enableLimit;
|
|
};
|
|
/**
|
|
* Enable/disable the joint limit.
|
|
*/
|
|
RevoluteJoint.prototype.enableLimit = function (flag) {
|
|
if (flag != this.m_enableLimit) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_enableLimit = flag;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
};
|
|
/**
|
|
* Get the lower joint limit in radians.
|
|
*/
|
|
RevoluteJoint.prototype.getLowerLimit = function () {
|
|
return this.m_lowerAngle;
|
|
};
|
|
/**
|
|
* Get the upper joint limit in radians.
|
|
*/
|
|
RevoluteJoint.prototype.getUpperLimit = function () {
|
|
return this.m_upperAngle;
|
|
};
|
|
/**
|
|
* Set the joint limits in radians.
|
|
*/
|
|
RevoluteJoint.prototype.setLimits = function (lower, upper) {
|
|
if (lower != this.m_lowerAngle || upper != this.m_upperAngle) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_impulse.z = 0.0;
|
|
this.m_lowerAngle = lower;
|
|
this.m_upperAngle = upper;
|
|
}
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
RevoluteJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
RevoluteJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force given the inverse time step. Unit is N.
|
|
*/
|
|
RevoluteJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.neo(this.m_impulse.x, this.m_impulse.y).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque due to the joint limit given the inverse time step.
|
|
* Unit is N*m.
|
|
*/
|
|
RevoluteJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_impulse.z;
|
|
};
|
|
RevoluteJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// J = [-I -r1_skew I r2_skew]
|
|
// [ 0 -1 0 1]
|
|
// r_skew = [-ry; rx]
|
|
// Matlab
|
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var fixedRotation = (iA + iB === 0.0); // bool
|
|
this.m_mass.ex.x = mA + mB + this.m_rA.y * this.m_rA.y * iA + this.m_rB.y
|
|
* this.m_rB.y * iB;
|
|
this.m_mass.ey.x = -this.m_rA.y * this.m_rA.x * iA - this.m_rB.y
|
|
* this.m_rB.x * iB;
|
|
this.m_mass.ez.x = -this.m_rA.y * iA - this.m_rB.y * iB;
|
|
this.m_mass.ex.y = this.m_mass.ey.x;
|
|
this.m_mass.ey.y = mA + mB + this.m_rA.x * this.m_rA.x * iA + this.m_rB.x
|
|
* this.m_rB.x * iB;
|
|
this.m_mass.ez.y = this.m_rA.x * iA + this.m_rB.x * iB;
|
|
this.m_mass.ex.z = this.m_mass.ez.x;
|
|
this.m_mass.ey.z = this.m_mass.ez.y;
|
|
this.m_mass.ez.z = iA + iB;
|
|
this.m_motorMass = iA + iB;
|
|
if (this.m_motorMass > 0.0) {
|
|
this.m_motorMass = 1.0 / this.m_motorMass;
|
|
}
|
|
if (this.m_enableMotor == false || fixedRotation) {
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
if (this.m_enableLimit && fixedRotation == false) {
|
|
var jointAngle = aB - aA - this.m_referenceAngle; // float
|
|
if (math.abs(this.m_upperAngle - this.m_lowerAngle) < 2.0 * Settings.angularSlop) {
|
|
this.m_limitState = equalLimits$1;
|
|
}
|
|
else if (jointAngle <= this.m_lowerAngle) {
|
|
if (this.m_limitState != atLowerLimit$1) {
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
this.m_limitState = atLowerLimit$1;
|
|
}
|
|
else if (jointAngle >= this.m_upperAngle) {
|
|
if (this.m_limitState != atUpperLimit$2) {
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
this.m_limitState = atUpperLimit$2;
|
|
}
|
|
else {
|
|
this.m_limitState = inactiveLimit$2;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
this.m_limitState = inactiveLimit$2;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Scale impulses to support a variable time step.
|
|
this.m_impulse.mul(step.dtRatio);
|
|
this.m_motorImpulse *= step.dtRatio;
|
|
var P = Vec2.neo(this.m_impulse.x, this.m_impulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + this.m_motorImpulse + this.m_impulse.z);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + this.m_motorImpulse + this.m_impulse.z);
|
|
}
|
|
else {
|
|
this.m_impulse.setZero();
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
RevoluteJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var fixedRotation = (iA + iB === 0.0); // bool
|
|
// Solve motor constraint.
|
|
if (this.m_enableMotor && this.m_limitState != equalLimits$1
|
|
&& fixedRotation == false) {
|
|
var Cdot = wB - wA - this.m_motorSpeed; // float
|
|
var impulse = -this.m_motorMass * Cdot; // float
|
|
var oldImpulse = this.m_motorImpulse; // float
|
|
var maxImpulse = step.dt * this.m_maxMotorTorque; // float
|
|
this.m_motorImpulse = math.clamp(this.m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
|
impulse = this.m_motorImpulse - oldImpulse;
|
|
wA -= iA * impulse;
|
|
wB += iB * impulse;
|
|
}
|
|
// Solve limit constraint.
|
|
if (this.m_enableLimit && this.m_limitState != inactiveLimit$2
|
|
&& fixedRotation == false) {
|
|
var Cdot1 = Vec2.zero();
|
|
Cdot1.addCombine(1, vB, 1, Vec2.cross(wB, this.m_rB));
|
|
Cdot1.subCombine(1, vA, 1, Vec2.cross(wA, this.m_rA));
|
|
var Cdot2 = wB - wA; // float
|
|
var Cdot = new Vec3(Cdot1.x, Cdot1.y, Cdot2);
|
|
var impulse = Vec3.neg(this.m_mass.solve33(Cdot)); // Vec3
|
|
if (this.m_limitState == equalLimits$1) {
|
|
this.m_impulse.add(impulse);
|
|
}
|
|
else if (this.m_limitState == atLowerLimit$1) {
|
|
var newImpulse = this.m_impulse.z + impulse.z; // float
|
|
if (newImpulse < 0.0) {
|
|
var rhs = Vec2.combine(-1, Cdot1, this.m_impulse.z, Vec2.neo(this.m_mass.ez.x, this.m_mass.ez.y)); // Vec2
|
|
var reduced = this.m_mass.solve22(rhs); // Vec2
|
|
impulse.x = reduced.x;
|
|
impulse.y = reduced.y;
|
|
impulse.z = -this.m_impulse.z;
|
|
this.m_impulse.x += reduced.x;
|
|
this.m_impulse.y += reduced.y;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
else {
|
|
this.m_impulse.add(impulse);
|
|
}
|
|
}
|
|
else if (this.m_limitState == atUpperLimit$2) {
|
|
var newImpulse = this.m_impulse.z + impulse.z; // float
|
|
if (newImpulse > 0.0) {
|
|
var rhs = Vec2.combine(-1, Cdot1, this.m_impulse.z, Vec2.neo(this.m_mass.ez.x, this.m_mass.ez.y)); // Vec2
|
|
var reduced = this.m_mass.solve22(rhs); // Vec2
|
|
impulse.x = reduced.x;
|
|
impulse.y = reduced.y;
|
|
impulse.z = -this.m_impulse.z;
|
|
this.m_impulse.x += reduced.x;
|
|
this.m_impulse.y += reduced.y;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
else {
|
|
this.m_impulse.add(impulse);
|
|
}
|
|
}
|
|
var P = Vec2.neo(impulse.x, impulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + impulse.z);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + impulse.z);
|
|
}
|
|
else {
|
|
// Solve point-to-point constraint
|
|
var Cdot = Vec2.zero();
|
|
Cdot.addCombine(1, vB, 1, Vec2.cross(wB, this.m_rB));
|
|
Cdot.subCombine(1, vA, 1, Vec2.cross(wA, this.m_rA));
|
|
var impulse = this.m_mass.solve22(Vec2.neg(Cdot)); // Vec2
|
|
this.m_impulse.x += impulse.x;
|
|
this.m_impulse.y += impulse.y;
|
|
vA.subMul(mA, impulse);
|
|
wA -= iA * Vec2.cross(this.m_rA, impulse);
|
|
vB.addMul(mB, impulse);
|
|
wB += iB * Vec2.cross(this.m_rB, impulse);
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
RevoluteJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var angularError = 0.0; // float
|
|
var positionError = 0.0; // float
|
|
var fixedRotation = (this.m_invIA + this.m_invIB == 0.0); // bool
|
|
// Solve angular limit constraint.
|
|
if (this.m_enableLimit && this.m_limitState != inactiveLimit$2
|
|
&& fixedRotation == false) {
|
|
var angle = aB - aA - this.m_referenceAngle; // float
|
|
var limitImpulse = 0.0; // float
|
|
if (this.m_limitState == equalLimits$1) {
|
|
// Prevent large angular corrections
|
|
var C = math.clamp(angle - this.m_lowerAngle, -Settings.maxAngularCorrection, Settings.maxAngularCorrection); // float
|
|
limitImpulse = -this.m_motorMass * C;
|
|
angularError = math.abs(C);
|
|
}
|
|
else if (this.m_limitState == atLowerLimit$1) {
|
|
var C = angle - this.m_lowerAngle; // float
|
|
angularError = -C;
|
|
// Prevent large angular corrections and allow some slop.
|
|
C = math.clamp(C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0);
|
|
limitImpulse = -this.m_motorMass * C;
|
|
}
|
|
else if (this.m_limitState == atUpperLimit$2) {
|
|
var C = angle - this.m_upperAngle; // float
|
|
angularError = C;
|
|
// Prevent large angular corrections and allow some slop.
|
|
C = math.clamp(C - Settings.angularSlop, 0.0, Settings.maxAngularCorrection);
|
|
limitImpulse = -this.m_motorMass * C;
|
|
}
|
|
aA -= this.m_invIA * limitImpulse;
|
|
aB += this.m_invIB * limitImpulse;
|
|
}
|
|
// Solve point-to-point constraint.
|
|
{
|
|
qA.set(aA);
|
|
qB.set(aB);
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA)); // Vec2
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB)); // Vec2
|
|
var C = Vec2.zero();
|
|
C.addCombine(1, cB, 1, rB);
|
|
C.subCombine(1, cA, 1, rA);
|
|
positionError = C.length();
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var K = new Mat22();
|
|
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
|
|
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
|
|
K.ey.x = K.ex.y;
|
|
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
|
|
var impulse = Vec2.neg(K.solve(C)); // Vec2
|
|
cA.subMul(mA, impulse);
|
|
aA -= iA * Vec2.cross(rA, impulse);
|
|
cB.addMul(mB, impulse);
|
|
aB += iB * Vec2.cross(rB, impulse);
|
|
}
|
|
this.m_bodyA.c_position.c.set(cA);
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c.set(cB);
|
|
this.m_bodyB.c_position.a = aB;
|
|
return positionError <= Settings.linearSlop
|
|
&& angularError <= Settings.angularSlop;
|
|
};
|
|
RevoluteJoint.TYPE = 'revolute-joint';
|
|
return RevoluteJoint;
|
|
}(Joint));
|
|
Joint.TYPES[RevoluteJoint.TYPE] = RevoluteJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var inactiveLimit$1 = 0;
|
|
var atLowerLimit = 1;
|
|
var atUpperLimit$1 = 2;
|
|
var equalLimits = 3;
|
|
var DEFAULTS$7 = {
|
|
enableLimit: false,
|
|
lowerTranslation: 0.0,
|
|
upperTranslation: 0.0,
|
|
enableMotor: false,
|
|
maxMotorForce: 0.0,
|
|
motorSpeed: 0.0
|
|
};
|
|
/**
|
|
* A prismatic joint. This joint provides one degree of freedom: translation
|
|
* along an axis fixed in bodyA. Relative rotation is prevented. You can use a
|
|
* joint limit to restrict the range of motion and a joint motor to drive the
|
|
* motion or to model joint friction.
|
|
*/
|
|
var PrismaticJoint = /** @class */ (function (_super) {
|
|
__extends(PrismaticJoint, _super);
|
|
function PrismaticJoint(def, bodyA, bodyB, anchor, axis) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof PrismaticJoint)) {
|
|
return new PrismaticJoint(def, bodyA, bodyB, anchor, axis);
|
|
}
|
|
def = options(def, DEFAULTS$7);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = PrismaticJoint.TYPE;
|
|
_this.m_localAnchorA = Vec2.clone(anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.zero());
|
|
_this.m_localXAxisA = Vec2.clone(axis ? bodyA.getLocalVector(axis) : def.localAxisA || Vec2.neo(1.0, 0.0));
|
|
_this.m_localXAxisA.normalize();
|
|
_this.m_localYAxisA = Vec2.cross(1.0, _this.m_localXAxisA);
|
|
_this.m_referenceAngle = math.isFinite(def.referenceAngle) ? def.referenceAngle : bodyB.getAngle() - bodyA.getAngle();
|
|
_this.m_impulse = new Vec3();
|
|
_this.m_motorMass = 0.0;
|
|
_this.m_motorImpulse = 0.0;
|
|
_this.m_lowerTranslation = def.lowerTranslation;
|
|
_this.m_upperTranslation = def.upperTranslation;
|
|
_this.m_maxMotorForce = def.maxMotorForce;
|
|
_this.m_motorSpeed = def.motorSpeed;
|
|
_this.m_enableLimit = def.enableLimit;
|
|
_this.m_enableMotor = def.enableMotor;
|
|
_this.m_limitState = inactiveLimit$1;
|
|
_this.m_axis = Vec2.zero();
|
|
_this.m_perp = Vec2.zero();
|
|
// Solver temp
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_axis, _this.m_perp; // Vec2
|
|
_this.m_s1, _this.m_s2; // float
|
|
_this.m_a1, _this.m_a2; // float
|
|
_this.m_K = new Mat33();
|
|
_this.m_motorMass; // float
|
|
return _this;
|
|
// Linear constraint (point-to-line)
|
|
// d = p2 - p1 = x2 + r2 - x1 - r1
|
|
// C = dot(perp, d)
|
|
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 -
|
|
// cross(w1, r1))
|
|
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) +
|
|
// dot(cross(r2, perp), v2)
|
|
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
|
|
//
|
|
// Angular constraint
|
|
// C = a2 - a1 + a_initial
|
|
// Cdot = w2 - w1
|
|
// J = [0 0 -1 0 0 1]
|
|
//
|
|
// K = J * invM * JT
|
|
//
|
|
// J = [-a -s1 a s2]
|
|
// [0 -1 0 1]
|
|
// a = perp
|
|
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
|
|
// s2 = cross(r2, a) = cross(p2 - x2, a)
|
|
// Motor/Limit linear constraint
|
|
// C = dot(ax1, d)
|
|
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) +
|
|
// dot(cross(r2, ax1), v2)
|
|
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
|
|
// Block Solver
|
|
// We develop a block solver that includes the joint limit. This makes the
|
|
// limit stiff (inelastic) even
|
|
// when the mass has poor distribution (leading to large torques about the
|
|
// joint anchor points).
|
|
//
|
|
// The Jacobian has 3 rows:
|
|
// J = [-uT -s1 uT s2] // linear
|
|
// [0 -1 0 1] // angular
|
|
// [-vT -a1 vT a2] // limit
|
|
//
|
|
// u = perp
|
|
// v = axis
|
|
// s1 = cross(d + r1, u), s2 = cross(r2, u)
|
|
// a1 = cross(d + r1, v), a2 = cross(r2, v)
|
|
// M * (v2 - v1) = JT * df
|
|
// J * v2 = bias
|
|
//
|
|
// v2 = v1 + invM * JT * df
|
|
// J * (v1 + invM * JT * df) = bias
|
|
// K * df = bias - J * v1 = -Cdot
|
|
// K = J * invM * JT
|
|
// Cdot = J * v1 - bias
|
|
//
|
|
// Now solve for f2.
|
|
// df = f2 - f1
|
|
// K * (f2 - f1) = -Cdot
|
|
// f2 = invK * (-Cdot) + f1
|
|
//
|
|
// Clamp accumulated limit impulse.
|
|
// lower: f2(3) = max(f2(3), 0)
|
|
// upper: f2(3) = min(f2(3), 0)
|
|
//
|
|
// Solve for correct f2(1:2)
|
|
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
|
|
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
|
|
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) +
|
|
// K(1:2,1:2) * f1(1:2)
|
|
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
|
|
// f1(1:2)
|
|
//
|
|
// Now compute impulse to be applied:
|
|
// df = f2 - f1
|
|
}
|
|
/** @internal */
|
|
PrismaticJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
lowerTranslation: this.m_lowerTranslation,
|
|
upperTranslation: this.m_upperTranslation,
|
|
maxMotorForce: this.m_maxMotorForce,
|
|
motorSpeed: this.m_motorSpeed,
|
|
enableLimit: this.m_enableLimit,
|
|
enableMotor: this.m_enableMotor,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
localAxisA: this.m_localXAxisA,
|
|
referenceAngle: this.m_referenceAngle,
|
|
};
|
|
};
|
|
/** @internal */
|
|
PrismaticJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
data.localAxisA = new Vec2(data.localAxisA);
|
|
var joint = new PrismaticJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
PrismaticJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
if (def.localAxisA) {
|
|
this.m_localXAxisA.set(def.localAxisA);
|
|
this.m_localYAxisA.set(Vec2.cross(1.0, def.localAxisA));
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
PrismaticJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
PrismaticJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* The local joint axis relative to bodyA.
|
|
*/
|
|
PrismaticJoint.prototype.getLocalAxisA = function () {
|
|
return this.m_localXAxisA;
|
|
};
|
|
/**
|
|
* Get the reference angle.
|
|
*/
|
|
PrismaticJoint.prototype.getReferenceAngle = function () {
|
|
return this.m_referenceAngle;
|
|
};
|
|
/**
|
|
* Get the current joint translation, usually in meters.
|
|
*/
|
|
PrismaticJoint.prototype.getJointTranslation = function () {
|
|
var pA = this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
var pB = this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
var d = Vec2.sub(pB, pA);
|
|
var axis = this.m_bodyA.getWorldVector(this.m_localXAxisA);
|
|
var translation = Vec2.dot(d, axis);
|
|
return translation;
|
|
};
|
|
/**
|
|
* Get the current joint translation speed, usually in meters per second.
|
|
*/
|
|
PrismaticJoint.prototype.getJointSpeed = function () {
|
|
var bA = this.m_bodyA;
|
|
var bB = this.m_bodyB;
|
|
var rA = Rot.mulVec2(bA.m_xf.q, Vec2.sub(this.m_localAnchorA, bA.m_sweep.localCenter)); // Vec2
|
|
var rB = Rot.mulVec2(bB.m_xf.q, Vec2.sub(this.m_localAnchorB, bB.m_sweep.localCenter)); // Vec2
|
|
var p1 = Vec2.add(bA.m_sweep.c, rA); // Vec2
|
|
var p2 = Vec2.add(bB.m_sweep.c, rB); // Vec2
|
|
var d = Vec2.sub(p2, p1); // Vec2
|
|
var axis = Rot.mulVec2(bA.m_xf.q, this.m_localXAxisA); // Vec2
|
|
var vA = bA.m_linearVelocity; // Vec2
|
|
var vB = bB.m_linearVelocity; // Vec2
|
|
var wA = bA.m_angularVelocity; // float
|
|
var wB = bB.m_angularVelocity; // float
|
|
var speed = Vec2.dot(d, Vec2.cross(wA, axis))
|
|
+ Vec2.dot(axis, Vec2.sub(Vec2.addCross(vB, wB, rB), Vec2.addCross(vA, wA, rA))); // float
|
|
return speed;
|
|
};
|
|
/**
|
|
* Is the joint limit enabled?
|
|
*/
|
|
PrismaticJoint.prototype.isLimitEnabled = function () {
|
|
return this.m_enableLimit;
|
|
};
|
|
/**
|
|
* Enable/disable the joint limit.
|
|
*/
|
|
PrismaticJoint.prototype.enableLimit = function (flag) {
|
|
if (flag != this.m_enableLimit) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_enableLimit = flag;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
};
|
|
/**
|
|
* Get the lower joint limit, usually in meters.
|
|
*/
|
|
PrismaticJoint.prototype.getLowerLimit = function () {
|
|
return this.m_lowerTranslation;
|
|
};
|
|
/**
|
|
* Get the upper joint limit, usually in meters.
|
|
*/
|
|
PrismaticJoint.prototype.getUpperLimit = function () {
|
|
return this.m_upperTranslation;
|
|
};
|
|
/**
|
|
* Set the joint limits, usually in meters.
|
|
*/
|
|
PrismaticJoint.prototype.setLimits = function (lower, upper) {
|
|
if (lower != this.m_lowerTranslation || upper != this.m_upperTranslation) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_lowerTranslation = lower;
|
|
this.m_upperTranslation = upper;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
};
|
|
/**
|
|
* Is the joint motor enabled?
|
|
*/
|
|
PrismaticJoint.prototype.isMotorEnabled = function () {
|
|
return this.m_enableMotor;
|
|
};
|
|
/**
|
|
* Enable/disable the joint motor.
|
|
*/
|
|
PrismaticJoint.prototype.enableMotor = function (flag) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_enableMotor = flag;
|
|
};
|
|
/**
|
|
* Set the motor speed, usually in meters per second.
|
|
*/
|
|
PrismaticJoint.prototype.setMotorSpeed = function (speed) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_motorSpeed = speed;
|
|
};
|
|
/**
|
|
* Set the maximum motor force, usually in N.
|
|
*/
|
|
PrismaticJoint.prototype.setMaxMotorForce = function (force) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_maxMotorForce = force;
|
|
};
|
|
PrismaticJoint.prototype.getMaxMotorForce = function () {
|
|
return this.m_maxMotorForce;
|
|
};
|
|
/**
|
|
* Get the motor speed, usually in meters per second.
|
|
*/
|
|
PrismaticJoint.prototype.getMotorSpeed = function () {
|
|
return this.m_motorSpeed;
|
|
};
|
|
/**
|
|
* Get the current motor force given the inverse time step, usually in N.
|
|
*/
|
|
PrismaticJoint.prototype.getMotorForce = function (inv_dt) {
|
|
return inv_dt * this.m_motorImpulse;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
PrismaticJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
PrismaticJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
PrismaticJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.combine(this.m_impulse.x, this.m_perp, this.m_motorImpulse + this.m_impulse.z, this.m_axis).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
PrismaticJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_impulse.y;
|
|
};
|
|
PrismaticJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
// Compute the effective masses.
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
var d = Vec2.zero();
|
|
d.addCombine(1, cB, 1, rB);
|
|
d.subCombine(1, cA, 1, rA);
|
|
var mA = this.m_invMassA, mB = this.m_invMassB;
|
|
var iA = this.m_invIA, iB = this.m_invIB;
|
|
// Compute motor Jacobian and effective mass.
|
|
{
|
|
this.m_axis = Rot.mulVec2(qA, this.m_localXAxisA);
|
|
this.m_a1 = Vec2.cross(Vec2.add(d, rA), this.m_axis);
|
|
this.m_a2 = Vec2.cross(rB, this.m_axis);
|
|
this.m_motorMass = mA + mB + iA * this.m_a1 * this.m_a1 + iB * this.m_a2
|
|
* this.m_a2;
|
|
if (this.m_motorMass > 0.0) {
|
|
this.m_motorMass = 1.0 / this.m_motorMass;
|
|
}
|
|
}
|
|
// Prismatic constraint.
|
|
{
|
|
this.m_perp = Rot.mulVec2(qA, this.m_localYAxisA);
|
|
this.m_s1 = Vec2.cross(Vec2.add(d, rA), this.m_perp);
|
|
this.m_s2 = Vec2.cross(rB, this.m_perp);
|
|
Vec2.cross(rA, this.m_perp);
|
|
var k11 = mA + mB + iA * this.m_s1 * this.m_s1 + iB * this.m_s2 * this.m_s2;
|
|
var k12 = iA * this.m_s1 + iB * this.m_s2;
|
|
var k13 = iA * this.m_s1 * this.m_a1 + iB * this.m_s2 * this.m_a2;
|
|
var k22 = iA + iB;
|
|
if (k22 == 0.0) {
|
|
// For bodies with fixed rotation.
|
|
k22 = 1.0;
|
|
}
|
|
var k23 = iA * this.m_a1 + iB * this.m_a2;
|
|
var k33 = mA + mB + iA * this.m_a1 * this.m_a1 + iB * this.m_a2 * this.m_a2;
|
|
this.m_K.ex.set(k11, k12, k13);
|
|
this.m_K.ey.set(k12, k22, k23);
|
|
this.m_K.ez.set(k13, k23, k33);
|
|
}
|
|
// Compute motor and limit terms.
|
|
if (this.m_enableLimit) {
|
|
var jointTranslation = Vec2.dot(this.m_axis, d); // float
|
|
if (math.abs(this.m_upperTranslation - this.m_lowerTranslation) < 2.0 * Settings.linearSlop) {
|
|
this.m_limitState = equalLimits;
|
|
}
|
|
else if (jointTranslation <= this.m_lowerTranslation) {
|
|
if (this.m_limitState != atLowerLimit) {
|
|
this.m_limitState = atLowerLimit;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
}
|
|
else if (jointTranslation >= this.m_upperTranslation) {
|
|
if (this.m_limitState != atUpperLimit$1) {
|
|
this.m_limitState = atUpperLimit$1;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
this.m_limitState = inactiveLimit$1;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
this.m_limitState = inactiveLimit$1;
|
|
this.m_impulse.z = 0.0;
|
|
}
|
|
if (this.m_enableMotor == false) {
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Account for variable time step.
|
|
this.m_impulse.mul(step.dtRatio);
|
|
this.m_motorImpulse *= step.dtRatio;
|
|
var P = Vec2.combine(this.m_impulse.x, this.m_perp, this.m_motorImpulse
|
|
+ this.m_impulse.z, this.m_axis);
|
|
var LA = this.m_impulse.x * this.m_s1 + this.m_impulse.y
|
|
+ (this.m_motorImpulse + this.m_impulse.z) * this.m_a1;
|
|
var LB = this.m_impulse.x * this.m_s2 + this.m_impulse.y
|
|
+ (this.m_motorImpulse + this.m_impulse.z) * this.m_a2;
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
else {
|
|
this.m_impulse.setZero();
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
PrismaticJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB;
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB;
|
|
// Solve linear motor constraint.
|
|
if (this.m_enableMotor && this.m_limitState != equalLimits) {
|
|
var Cdot = Vec2.dot(this.m_axis, Vec2.sub(vB, vA)) + this.m_a2 * wB
|
|
- this.m_a1 * wA;
|
|
var impulse = this.m_motorMass * (this.m_motorSpeed - Cdot);
|
|
var oldImpulse = this.m_motorImpulse;
|
|
var maxImpulse = step.dt * this.m_maxMotorForce;
|
|
this.m_motorImpulse = math.clamp(this.m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
|
impulse = this.m_motorImpulse - oldImpulse;
|
|
var P = Vec2.mul(impulse, this.m_axis);
|
|
var LA = impulse * this.m_a1;
|
|
var LB = impulse * this.m_a2;
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
var Cdot1 = Vec2.zero();
|
|
Cdot1.x += Vec2.dot(this.m_perp, vB) + this.m_s2 * wB;
|
|
Cdot1.x -= Vec2.dot(this.m_perp, vA) + this.m_s1 * wA;
|
|
Cdot1.y = wB - wA;
|
|
if (this.m_enableLimit && this.m_limitState != inactiveLimit$1) {
|
|
// Solve prismatic and limit constraint in block form.
|
|
var Cdot2 = 0;
|
|
Cdot2 += Vec2.dot(this.m_axis, vB) + this.m_a2 * wB;
|
|
Cdot2 -= Vec2.dot(this.m_axis, vA) + this.m_a1 * wA;
|
|
var Cdot = new Vec3(Cdot1.x, Cdot1.y, Cdot2);
|
|
var f1 = new Vec3(this.m_impulse);
|
|
var df = this.m_K.solve33(Vec3.neg(Cdot)); // Vec3
|
|
this.m_impulse.add(df);
|
|
if (this.m_limitState == atLowerLimit) {
|
|
this.m_impulse.z = math.max(this.m_impulse.z, 0.0);
|
|
}
|
|
else if (this.m_limitState == atUpperLimit$1) {
|
|
this.m_impulse.z = math.min(this.m_impulse.z, 0.0);
|
|
}
|
|
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
|
|
// f1(1:2)
|
|
var b = Vec2.combine(-1, Cdot1, -(this.m_impulse.z - f1.z), Vec2.neo(this.m_K.ez.x, this.m_K.ez.y)); // Vec2
|
|
var f2r = Vec2.add(this.m_K.solve22(b), Vec2.neo(f1.x, f1.y)); // Vec2
|
|
this.m_impulse.x = f2r.x;
|
|
this.m_impulse.y = f2r.y;
|
|
df = Vec3.sub(this.m_impulse, f1);
|
|
var P = Vec2.combine(df.x, this.m_perp, df.z, this.m_axis); // Vec2
|
|
var LA = df.x * this.m_s1 + df.y + df.z * this.m_a1; // float
|
|
var LB = df.x * this.m_s2 + df.y + df.z * this.m_a2; // float
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
else {
|
|
// Limit is inactive, just solve the prismatic constraint in block form.
|
|
var df = this.m_K.solve22(Vec2.neg(Cdot1)); // Vec2
|
|
this.m_impulse.x += df.x;
|
|
this.m_impulse.y += df.y;
|
|
var P = Vec2.mul(df.x, this.m_perp); // Vec2
|
|
var LA = df.x * this.m_s1 + df.y; // float
|
|
var LB = df.x * this.m_s2 + df.y; // float
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
PrismaticJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB;
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB;
|
|
// Compute fresh Jacobians
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA)); // Vec2
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB)); // Vec2
|
|
var d = Vec2.sub(Vec2.add(cB, rB), Vec2.add(cA, rA)); // Vec2
|
|
var axis = Rot.mulVec2(qA, this.m_localXAxisA); // Vec2
|
|
var a1 = Vec2.cross(Vec2.add(d, rA), axis); // float
|
|
var a2 = Vec2.cross(rB, axis); // float
|
|
var perp = Rot.mulVec2(qA, this.m_localYAxisA); // Vec2
|
|
var s1 = Vec2.cross(Vec2.add(d, rA), perp); // float
|
|
var s2 = Vec2.cross(rB, perp); // float
|
|
var impulse = new Vec3();
|
|
var C1 = Vec2.zero(); // Vec2
|
|
C1.x = Vec2.dot(perp, d);
|
|
C1.y = aB - aA - this.m_referenceAngle;
|
|
var linearError = math.abs(C1.x); // float
|
|
var angularError = math.abs(C1.y); // float
|
|
var linearSlop = Settings.linearSlop;
|
|
var maxLinearCorrection = Settings.maxLinearCorrection;
|
|
var active = false; // bool
|
|
var C2 = 0.0; // float
|
|
if (this.m_enableLimit) {
|
|
var translation = Vec2.dot(axis, d); // float
|
|
if (math.abs(this.m_upperTranslation - this.m_lowerTranslation) < 2.0 * linearSlop) {
|
|
// Prevent large angular corrections
|
|
C2 = math.clamp(translation, -maxLinearCorrection, maxLinearCorrection);
|
|
linearError = math.max(linearError, math.abs(translation));
|
|
active = true;
|
|
}
|
|
else if (translation <= this.m_lowerTranslation) {
|
|
// Prevent large linear corrections and allow some slop.
|
|
C2 = math.clamp(translation - this.m_lowerTranslation + linearSlop, -maxLinearCorrection, 0.0);
|
|
linearError = math
|
|
.max(linearError, this.m_lowerTranslation - translation);
|
|
active = true;
|
|
}
|
|
else if (translation >= this.m_upperTranslation) {
|
|
// Prevent large linear corrections and allow some slop.
|
|
C2 = math.clamp(translation - this.m_upperTranslation - linearSlop, 0.0, maxLinearCorrection);
|
|
linearError = math
|
|
.max(linearError, translation - this.m_upperTranslation);
|
|
active = true;
|
|
}
|
|
}
|
|
if (active) {
|
|
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; // float
|
|
var k12 = iA * s1 + iB * s2; // float
|
|
var k13 = iA * s1 * a1 + iB * s2 * a2; // float
|
|
var k22 = iA + iB; // float
|
|
if (k22 == 0.0) {
|
|
// For fixed rotation
|
|
k22 = 1.0;
|
|
}
|
|
var k23 = iA * a1 + iB * a2; // float
|
|
var k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; // float
|
|
var K = new Mat33();
|
|
K.ex.set(k11, k12, k13);
|
|
K.ey.set(k12, k22, k23);
|
|
K.ez.set(k13, k23, k33);
|
|
var C = new Vec3();
|
|
C.x = C1.x;
|
|
C.y = C1.y;
|
|
C.z = C2;
|
|
impulse = K.solve33(Vec3.neg(C));
|
|
}
|
|
else {
|
|
var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; // float
|
|
var k12 = iA * s1 + iB * s2; // float
|
|
var k22 = iA + iB; // float
|
|
if (k22 == 0.0) {
|
|
k22 = 1.0;
|
|
}
|
|
var K = new Mat22();
|
|
K.ex.set(k11, k12);
|
|
K.ey.set(k12, k22);
|
|
var impulse1 = K.solve(Vec2.neg(C1)); // Vec2
|
|
impulse.x = impulse1.x;
|
|
impulse.y = impulse1.y;
|
|
impulse.z = 0.0;
|
|
}
|
|
var P = Vec2.combine(impulse.x, perp, impulse.z, axis); // Vec2
|
|
var LA = impulse.x * s1 + impulse.y + impulse.z * a1; // float
|
|
var LB = impulse.x * s2 + impulse.y + impulse.z * a2; // float
|
|
cA.subMul(mA, P);
|
|
aA -= iA * LA;
|
|
cB.addMul(mB, P);
|
|
aB += iB * LB;
|
|
this.m_bodyA.c_position.c = cA;
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c = cB;
|
|
this.m_bodyB.c_position.a = aB;
|
|
return linearError <= Settings.linearSlop
|
|
&& angularError <= Settings.angularSlop;
|
|
};
|
|
PrismaticJoint.TYPE = 'prismatic-joint';
|
|
return PrismaticJoint;
|
|
}(Joint));
|
|
Joint.TYPES[PrismaticJoint.TYPE] = PrismaticJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$6 = {
|
|
ratio: 1.0
|
|
};
|
|
/**
|
|
* A gear joint is used to connect two joints together. Either joint can be a
|
|
* revolute or prismatic joint. You specify a gear ratio to bind the motions
|
|
* together: coordinate1 + ratio * coordinate2 = constant
|
|
*
|
|
* The ratio can be negative or positive. If one joint is a revolute joint and
|
|
* the other joint is a prismatic joint, then the ratio will have units of
|
|
* length or units of 1/length. Warning: You have to manually destroy the gear
|
|
* joint if joint1 or joint2 is destroyed.
|
|
*
|
|
* This definition requires two existing revolute or prismatic joints (any
|
|
* combination will work).
|
|
*/
|
|
var GearJoint = /** @class */ (function (_super) {
|
|
__extends(GearJoint, _super);
|
|
function GearJoint(def, bodyA, bodyB, joint1, joint2, ratio) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof GearJoint)) {
|
|
return new GearJoint(def, bodyA, bodyB, joint1, joint2, ratio);
|
|
}
|
|
def = options(def, DEFAULTS$6);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = GearJoint.TYPE;
|
|
_this.m_joint1 = joint1 ? joint1 : def.joint1;
|
|
_this.m_joint2 = joint2 ? joint2 : def.joint2;
|
|
_this.m_ratio = math.isFinite(ratio) ? ratio : def.ratio;
|
|
_this.m_type1 = _this.m_joint1.getType();
|
|
_this.m_type2 = _this.m_joint2.getType();
|
|
// joint1 connects body A to body C
|
|
// joint2 connects body B to body D
|
|
var coordinateA, coordinateB; // float
|
|
// TODO_ERIN there might be some problem with the joint edges in Joint.
|
|
_this.m_bodyC = _this.m_joint1.getBodyA();
|
|
_this.m_bodyA = _this.m_joint1.getBodyB();
|
|
// Get geometry of joint1
|
|
var xfA = _this.m_bodyA.m_xf;
|
|
var aA = _this.m_bodyA.m_sweep.a;
|
|
var xfC = _this.m_bodyC.m_xf;
|
|
var aC = _this.m_bodyC.m_sweep.a;
|
|
if (_this.m_type1 === RevoluteJoint.TYPE) {
|
|
var revolute = _this.m_joint1;
|
|
_this.m_localAnchorC = revolute.m_localAnchorA;
|
|
_this.m_localAnchorA = revolute.m_localAnchorB;
|
|
_this.m_referenceAngleA = revolute.m_referenceAngle;
|
|
_this.m_localAxisC = Vec2.zero();
|
|
coordinateA = aA - aC - _this.m_referenceAngleA;
|
|
}
|
|
else {
|
|
var prismatic = _this.m_joint1;
|
|
_this.m_localAnchorC = prismatic.m_localAnchorA;
|
|
_this.m_localAnchorA = prismatic.m_localAnchorB;
|
|
_this.m_referenceAngleA = prismatic.m_referenceAngle;
|
|
_this.m_localAxisC = prismatic.m_localXAxisA;
|
|
var pC = _this.m_localAnchorC;
|
|
var pA = Rot.mulTVec2(xfC.q, Vec2.add(Rot.mul(xfA.q, _this.m_localAnchorA), Vec2.sub(xfA.p, xfC.p)));
|
|
coordinateA = Vec2.dot(pA, _this.m_localAxisC) - Vec2.dot(pC, _this.m_localAxisC);
|
|
}
|
|
_this.m_bodyD = _this.m_joint2.getBodyA();
|
|
_this.m_bodyB = _this.m_joint2.getBodyB();
|
|
// Get geometry of joint2
|
|
var xfB = _this.m_bodyB.m_xf;
|
|
var aB = _this.m_bodyB.m_sweep.a;
|
|
var xfD = _this.m_bodyD.m_xf;
|
|
var aD = _this.m_bodyD.m_sweep.a;
|
|
if (_this.m_type2 === RevoluteJoint.TYPE) {
|
|
var revolute = _this.m_joint2;
|
|
_this.m_localAnchorD = revolute.m_localAnchorA;
|
|
_this.m_localAnchorB = revolute.m_localAnchorB;
|
|
_this.m_referenceAngleB = revolute.m_referenceAngle;
|
|
_this.m_localAxisD = Vec2.zero();
|
|
coordinateB = aB - aD - _this.m_referenceAngleB;
|
|
}
|
|
else {
|
|
var prismatic = _this.m_joint2;
|
|
_this.m_localAnchorD = prismatic.m_localAnchorA;
|
|
_this.m_localAnchorB = prismatic.m_localAnchorB;
|
|
_this.m_referenceAngleB = prismatic.m_referenceAngle;
|
|
_this.m_localAxisD = prismatic.m_localXAxisA;
|
|
var pD = _this.m_localAnchorD;
|
|
var pB = Rot.mulTVec2(xfD.q, Vec2.add(Rot.mul(xfB.q, _this.m_localAnchorB), Vec2.sub(xfB.p, xfD.p)));
|
|
coordinateB = Vec2.dot(pB, _this.m_localAxisD) - Vec2.dot(pD, _this.m_localAxisD);
|
|
}
|
|
_this.m_constant = coordinateA + _this.m_ratio * coordinateB;
|
|
_this.m_impulse = 0.0;
|
|
// Solver temp
|
|
_this.m_lcA, _this.m_lcB, _this.m_lcC, _this.m_lcD; // Vec2
|
|
_this.m_mA, _this.m_mB, _this.m_mC, _this.m_mD; // float
|
|
_this.m_iA, _this.m_iB, _this.m_iC, _this.m_iD; // float
|
|
_this.m_JvAC, _this.m_JvBD; // Vec2
|
|
_this.m_JwA, _this.m_JwB, _this.m_JwC, _this.m_JwD; // float
|
|
_this.m_mass; // float
|
|
return _this;
|
|
// Gear Joint:
|
|
// C0 = (coordinate1 + ratio * coordinate2)_initial
|
|
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
|
|
// J = [J1 ratio * J2]
|
|
// K = J * invM * JT
|
|
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
|
|
//
|
|
// Revolute:
|
|
// coordinate = rotation
|
|
// Cdot = angularVelocity
|
|
// J = [0 0 1]
|
|
// K = J * invM * JT = invI
|
|
//
|
|
// Prismatic:
|
|
// coordinate = dot(p - pg, ug)
|
|
// Cdot = dot(v + cross(w, r), ug)
|
|
// J = [ug cross(r, ug)]
|
|
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
|
|
}
|
|
/** @internal */
|
|
GearJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
joint1: this.m_joint1,
|
|
joint2: this.m_joint2,
|
|
ratio: this.m_ratio,
|
|
// _constant: this.m_constant,
|
|
};
|
|
};
|
|
/** @internal */
|
|
GearJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
data.joint1 = restore(Joint, data.joint1, world);
|
|
data.joint2 = restore(Joint, data.joint2, world);
|
|
var joint = new GearJoint(data);
|
|
// if (data._constant) joint.m_constant = data._constant;
|
|
return joint;
|
|
};
|
|
/**
|
|
* Get the first joint.
|
|
*/
|
|
GearJoint.prototype.getJoint1 = function () {
|
|
return this.m_joint1;
|
|
};
|
|
/**
|
|
* Get the second joint.
|
|
*/
|
|
GearJoint.prototype.getJoint2 = function () {
|
|
return this.m_joint2;
|
|
};
|
|
/**
|
|
* Set the gear ratio.
|
|
*/
|
|
GearJoint.prototype.setRatio = function (ratio) {
|
|
this.m_ratio = ratio;
|
|
};
|
|
/**
|
|
* Get the gear ratio.
|
|
*/
|
|
GearJoint.prototype.getRatio = function () {
|
|
return this.m_ratio;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
GearJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
GearJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
GearJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(this.m_impulse, this.m_JvAC).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
GearJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
var L = this.m_impulse * this.m_JwA; // float
|
|
return inv_dt * L;
|
|
};
|
|
GearJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_lcA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_lcB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_lcC = this.m_bodyC.m_sweep.localCenter;
|
|
this.m_lcD = this.m_bodyD.m_sweep.localCenter;
|
|
this.m_mA = this.m_bodyA.m_invMass;
|
|
this.m_mB = this.m_bodyB.m_invMass;
|
|
this.m_mC = this.m_bodyC.m_invMass;
|
|
this.m_mD = this.m_bodyD.m_invMass;
|
|
this.m_iA = this.m_bodyA.m_invI;
|
|
this.m_iB = this.m_bodyB.m_invI;
|
|
this.m_iC = this.m_bodyC.m_invI;
|
|
this.m_iD = this.m_bodyD.m_invI;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var aC = this.m_bodyC.c_position.a;
|
|
var vC = this.m_bodyC.c_velocity.v;
|
|
var wC = this.m_bodyC.c_velocity.w;
|
|
var aD = this.m_bodyD.c_position.a;
|
|
var vD = this.m_bodyD.c_velocity.v;
|
|
var wD = this.m_bodyD.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var qC = Rot.neo(aC);
|
|
var qD = Rot.neo(aD);
|
|
this.m_mass = 0.0;
|
|
if (this.m_type1 == RevoluteJoint.TYPE) {
|
|
this.m_JvAC = Vec2.zero();
|
|
this.m_JwA = 1.0;
|
|
this.m_JwC = 1.0;
|
|
this.m_mass += this.m_iA + this.m_iC;
|
|
}
|
|
else {
|
|
var u = Rot.mulVec2(qC, this.m_localAxisC); // Vec2
|
|
var rC = Rot.mulSub(qC, this.m_localAnchorC, this.m_lcC); // Vec2
|
|
var rA = Rot.mulSub(qA, this.m_localAnchorA, this.m_lcA); // Vec2
|
|
this.m_JvAC = u;
|
|
this.m_JwC = Vec2.cross(rC, u);
|
|
this.m_JwA = Vec2.cross(rA, u);
|
|
this.m_mass += this.m_mC + this.m_mA + this.m_iC * this.m_JwC * this.m_JwC + this.m_iA * this.m_JwA * this.m_JwA;
|
|
}
|
|
if (this.m_type2 == RevoluteJoint.TYPE) {
|
|
this.m_JvBD = Vec2.zero();
|
|
this.m_JwB = this.m_ratio;
|
|
this.m_JwD = this.m_ratio;
|
|
this.m_mass += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
|
|
}
|
|
else {
|
|
var u = Rot.mulVec2(qD, this.m_localAxisD); // Vec2
|
|
var rD = Rot.mulSub(qD, this.m_localAnchorD, this.m_lcD); // Vec2
|
|
var rB = Rot.mulSub(qB, this.m_localAnchorB, this.m_lcB); // Vec2
|
|
this.m_JvBD = Vec2.mul(this.m_ratio, u);
|
|
this.m_JwD = this.m_ratio * Vec2.cross(rD, u);
|
|
this.m_JwB = this.m_ratio * Vec2.cross(rB, u);
|
|
this.m_mass += this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) + this.m_iD * this.m_JwD * this.m_JwD + this.m_iB * this.m_JwB * this.m_JwB;
|
|
}
|
|
// Compute effective mass.
|
|
this.m_mass = this.m_mass > 0.0 ? 1.0 / this.m_mass : 0.0;
|
|
if (step.warmStarting) {
|
|
vA.addMul(this.m_mA * this.m_impulse, this.m_JvAC);
|
|
wA += this.m_iA * this.m_impulse * this.m_JwA;
|
|
vB.addMul(this.m_mB * this.m_impulse, this.m_JvBD);
|
|
wB += this.m_iB * this.m_impulse * this.m_JwB;
|
|
vC.subMul(this.m_mC * this.m_impulse, this.m_JvAC);
|
|
wC -= this.m_iC * this.m_impulse * this.m_JwC;
|
|
vD.subMul(this.m_mD * this.m_impulse, this.m_JvBD);
|
|
wD -= this.m_iD * this.m_impulse * this.m_JwD;
|
|
}
|
|
else {
|
|
this.m_impulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
this.m_bodyC.c_velocity.v.set(vC);
|
|
this.m_bodyC.c_velocity.w = wC;
|
|
this.m_bodyD.c_velocity.v.set(vD);
|
|
this.m_bodyD.c_velocity.w = wD;
|
|
};
|
|
GearJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var vC = this.m_bodyC.c_velocity.v;
|
|
var wC = this.m_bodyC.c_velocity.w;
|
|
var vD = this.m_bodyD.c_velocity.v;
|
|
var wD = this.m_bodyD.c_velocity.w;
|
|
var Cdot = Vec2.dot(this.m_JvAC, vA) - Vec2.dot(this.m_JvAC, vC)
|
|
+ Vec2.dot(this.m_JvBD, vB) - Vec2.dot(this.m_JvBD, vD); // float
|
|
Cdot += (this.m_JwA * wA - this.m_JwC * wC)
|
|
+ (this.m_JwB * wB - this.m_JwD * wD);
|
|
var impulse = -this.m_mass * Cdot; // float
|
|
this.m_impulse += impulse;
|
|
vA.addMul(this.m_mA * impulse, this.m_JvAC);
|
|
wA += this.m_iA * impulse * this.m_JwA;
|
|
vB.addMul(this.m_mB * impulse, this.m_JvBD);
|
|
wB += this.m_iB * impulse * this.m_JwB;
|
|
vC.subMul(this.m_mC * impulse, this.m_JvAC);
|
|
wC -= this.m_iC * impulse * this.m_JwC;
|
|
vD.subMul(this.m_mD * impulse, this.m_JvBD);
|
|
wD -= this.m_iD * impulse * this.m_JwD;
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
this.m_bodyC.c_velocity.v.set(vC);
|
|
this.m_bodyC.c_velocity.w = wC;
|
|
this.m_bodyD.c_velocity.v.set(vD);
|
|
this.m_bodyD.c_velocity.w = wD;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
GearJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var cC = this.m_bodyC.c_position.c;
|
|
var aC = this.m_bodyC.c_position.a;
|
|
var cD = this.m_bodyD.c_position.c;
|
|
var aD = this.m_bodyD.c_position.a;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var qC = Rot.neo(aC);
|
|
var qD = Rot.neo(aD);
|
|
var linearError = 0.0; // float
|
|
var coordinateA, coordinateB; // float
|
|
var JvAC, JvBD; // Vec2
|
|
var JwA, JwB, JwC, JwD; // float
|
|
var mass = 0.0; // float
|
|
if (this.m_type1 == RevoluteJoint.TYPE) {
|
|
JvAC = Vec2.zero();
|
|
JwA = 1.0;
|
|
JwC = 1.0;
|
|
mass += this.m_iA + this.m_iC;
|
|
coordinateA = aA - aC - this.m_referenceAngleA;
|
|
}
|
|
else {
|
|
var u = Rot.mulVec2(qC, this.m_localAxisC); // Vec2
|
|
var rC = Rot.mulSub(qC, this.m_localAnchorC, this.m_lcC); // Vec2
|
|
var rA = Rot.mulSub(qA, this.m_localAnchorA, this.m_lcA); // Vec2
|
|
JvAC = u;
|
|
JwC = Vec2.cross(rC, u);
|
|
JwA = Vec2.cross(rA, u);
|
|
mass += this.m_mC + this.m_mA + this.m_iC * JwC * JwC + this.m_iA * JwA * JwA;
|
|
var pC = Vec2.sub(this.m_localAnchorC, this.m_lcC); // Vec2
|
|
var pA = Rot.mulTVec2(qC, Vec2.add(rA, Vec2.sub(cA, cC))); // Vec2
|
|
coordinateA = Vec2.dot(Vec2.sub(pA, pC), this.m_localAxisC);
|
|
}
|
|
if (this.m_type2 == RevoluteJoint.TYPE) {
|
|
JvBD = Vec2.zero();
|
|
JwB = this.m_ratio;
|
|
JwD = this.m_ratio;
|
|
mass += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
|
|
coordinateB = aB - aD - this.m_referenceAngleB;
|
|
}
|
|
else {
|
|
var u = Rot.mulVec2(qD, this.m_localAxisD);
|
|
var rD = Rot.mulSub(qD, this.m_localAnchorD, this.m_lcD);
|
|
var rB = Rot.mulSub(qB, this.m_localAnchorB, this.m_lcB);
|
|
JvBD = Vec2.mul(this.m_ratio, u);
|
|
JwD = this.m_ratio * Vec2.cross(rD, u);
|
|
JwB = this.m_ratio * Vec2.cross(rB, u);
|
|
mass += this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) + this.m_iD
|
|
* JwD * JwD + this.m_iB * JwB * JwB;
|
|
var pD = Vec2.sub(this.m_localAnchorD, this.m_lcD); // Vec2
|
|
var pB = Rot.mulTVec2(qD, Vec2.add(rB, Vec2.sub(cB, cD))); // Vec2
|
|
coordinateB = Vec2.dot(pB, this.m_localAxisD)
|
|
- Vec2.dot(pD, this.m_localAxisD);
|
|
}
|
|
var C = (coordinateA + this.m_ratio * coordinateB) - this.m_constant; // float
|
|
var impulse = 0.0; // float
|
|
if (mass > 0.0) {
|
|
impulse = -C / mass;
|
|
}
|
|
cA.addMul(this.m_mA * impulse, JvAC);
|
|
aA += this.m_iA * impulse * JwA;
|
|
cB.addMul(this.m_mB * impulse, JvBD);
|
|
aB += this.m_iB * impulse * JwB;
|
|
cC.subMul(this.m_mC * impulse, JvAC);
|
|
aC -= this.m_iC * impulse * JwC;
|
|
cD.subMul(this.m_mD * impulse, JvBD);
|
|
aD -= this.m_iD * impulse * JwD;
|
|
this.m_bodyA.c_position.c.set(cA);
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c.set(cB);
|
|
this.m_bodyB.c_position.a = aB;
|
|
this.m_bodyC.c_position.c.set(cC);
|
|
this.m_bodyC.c_position.a = aC;
|
|
this.m_bodyD.c_position.c.set(cD);
|
|
this.m_bodyD.c_position.a = aD;
|
|
// TODO_ERIN not implemented
|
|
return linearError < Settings.linearSlop;
|
|
};
|
|
GearJoint.TYPE = 'gear-joint';
|
|
return GearJoint;
|
|
}(Joint));
|
|
Joint.TYPES[GearJoint.TYPE] = GearJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$5 = {
|
|
maxForce: 1.0,
|
|
maxTorque: 1.0,
|
|
correctionFactor: 0.3
|
|
};
|
|
/**
|
|
* A motor joint is used to control the relative motion between two bodies. A
|
|
* typical usage is to control the movement of a dynamic body with respect to
|
|
* the ground.
|
|
*/
|
|
var MotorJoint = /** @class */ (function (_super) {
|
|
__extends(MotorJoint, _super);
|
|
function MotorJoint(def, bodyA, bodyB) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof MotorJoint)) {
|
|
return new MotorJoint(def, bodyA, bodyB);
|
|
}
|
|
def = options(def, DEFAULTS$5);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = MotorJoint.TYPE;
|
|
_this.m_linearOffset = math.isFinite(def.linearOffset) ? def.linearOffset : bodyA.getLocalPoint(bodyB.getPosition());
|
|
_this.m_angularOffset = math.isFinite(def.angularOffset) ? def.angularOffset : bodyB.getAngle() - bodyA.getAngle();
|
|
_this.m_linearImpulse = Vec2.zero();
|
|
_this.m_angularImpulse = 0.0;
|
|
_this.m_maxForce = def.maxForce;
|
|
_this.m_maxTorque = def.maxTorque;
|
|
_this.m_correctionFactor = def.correctionFactor;
|
|
// Solver temp
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_linearError; // Vec2
|
|
_this.m_angularError; // float
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_linearMass; // Mat22
|
|
_this.m_angularMass; // float
|
|
return _this;
|
|
// Point-to-point constraint
|
|
// Cdot = v2 - v1
|
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
|
// J = [-I -r1_skew I r2_skew ]
|
|
// Identity used:
|
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
|
// Angle constraint
|
|
// Cdot = w2 - w1
|
|
// J = [0 0 -1 0 0 1]
|
|
// K = invI1 + invI2
|
|
}
|
|
/** @internal */
|
|
MotorJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
maxForce: this.m_maxForce,
|
|
maxTorque: this.m_maxTorque,
|
|
correctionFactor: this.m_correctionFactor,
|
|
linearOffset: this.m_linearOffset,
|
|
angularOffset: this.m_angularOffset,
|
|
};
|
|
};
|
|
/** @internal */
|
|
MotorJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new MotorJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
MotorJoint.prototype._setAnchors = function (def) {
|
|
};
|
|
/**
|
|
* Set the maximum friction force in N.
|
|
*/
|
|
MotorJoint.prototype.setMaxForce = function (force) {
|
|
this.m_maxForce = force;
|
|
};
|
|
/**
|
|
* Get the maximum friction force in N.
|
|
*/
|
|
MotorJoint.prototype.getMaxForce = function () {
|
|
return this.m_maxForce;
|
|
};
|
|
/**
|
|
* Set the maximum friction torque in N*m.
|
|
*/
|
|
MotorJoint.prototype.setMaxTorque = function (torque) {
|
|
this.m_maxTorque = torque;
|
|
};
|
|
/**
|
|
* Get the maximum friction torque in N*m.
|
|
*/
|
|
MotorJoint.prototype.getMaxTorque = function () {
|
|
return this.m_maxTorque;
|
|
};
|
|
/**
|
|
* Set the position correction factor in the range [0,1].
|
|
*/
|
|
MotorJoint.prototype.setCorrectionFactor = function (factor) {
|
|
this.m_correctionFactor = factor;
|
|
};
|
|
/**
|
|
* Get the position correction factor in the range [0,1].
|
|
*/
|
|
MotorJoint.prototype.getCorrectionFactor = function () {
|
|
return this.m_correctionFactor;
|
|
};
|
|
/**
|
|
* Set/get the target linear offset, in frame A, in meters.
|
|
*/
|
|
MotorJoint.prototype.setLinearOffset = function (linearOffset) {
|
|
if (linearOffset.x != this.m_linearOffset.x
|
|
|| linearOffset.y != this.m_linearOffset.y) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_linearOffset = linearOffset;
|
|
}
|
|
};
|
|
MotorJoint.prototype.getLinearOffset = function () {
|
|
return this.m_linearOffset;
|
|
};
|
|
/**
|
|
* Set/get the target angular offset, in radians.
|
|
*/
|
|
MotorJoint.prototype.setAngularOffset = function (angularOffset) {
|
|
if (angularOffset != this.m_angularOffset) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_angularOffset = angularOffset;
|
|
}
|
|
};
|
|
MotorJoint.prototype.getAngularOffset = function () {
|
|
return this.m_angularOffset;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
MotorJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getPosition();
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
MotorJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getPosition();
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
MotorJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(inv_dt, this.m_linearImpulse);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
MotorJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_angularImpulse;
|
|
};
|
|
MotorJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA), qB = Rot.neo(aB);
|
|
// Compute the effective mass matrix.
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.neg(this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.neg(this.m_localCenterB));
|
|
// J = [-I -r1_skew I r2_skew]
|
|
// [ 0 -1 0 1]
|
|
// r_skew = [-ry; rx]
|
|
// Matlab
|
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB;
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB;
|
|
var K = new Mat22();
|
|
K.ex.x = mA + mB + iA * this.m_rA.y * this.m_rA.y + iB * this.m_rB.y
|
|
* this.m_rB.y;
|
|
K.ex.y = -iA * this.m_rA.x * this.m_rA.y - iB * this.m_rB.x * this.m_rB.y;
|
|
K.ey.x = K.ex.y;
|
|
K.ey.y = mA + mB + iA * this.m_rA.x * this.m_rA.x + iB * this.m_rB.x
|
|
* this.m_rB.x;
|
|
this.m_linearMass = K.getInverse();
|
|
this.m_angularMass = iA + iB;
|
|
if (this.m_angularMass > 0.0) {
|
|
this.m_angularMass = 1.0 / this.m_angularMass;
|
|
}
|
|
this.m_linearError = Vec2.zero();
|
|
this.m_linearError.addCombine(1, cB, 1, this.m_rB);
|
|
this.m_linearError.subCombine(1, cA, 1, this.m_rA);
|
|
this.m_linearError.sub(Rot.mulVec2(qA, this.m_linearOffset));
|
|
this.m_angularError = aB - aA - this.m_angularOffset;
|
|
if (step.warmStarting) {
|
|
// Scale impulses to support a variable time step.
|
|
this.m_linearImpulse.mul(step.dtRatio);
|
|
this.m_angularImpulse *= step.dtRatio;
|
|
var P = Vec2.neo(this.m_linearImpulse.x, this.m_linearImpulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + this.m_angularImpulse);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + this.m_angularImpulse);
|
|
}
|
|
else {
|
|
this.m_linearImpulse.setZero();
|
|
this.m_angularImpulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
MotorJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var mA = this.m_invMassA, mB = this.m_invMassB;
|
|
var iA = this.m_invIA, iB = this.m_invIB;
|
|
var h = step.dt;
|
|
var inv_h = step.inv_dt;
|
|
// Solve angular friction
|
|
{
|
|
var Cdot = wB - wA + inv_h * this.m_correctionFactor * this.m_angularError;
|
|
var impulse = -this.m_angularMass * Cdot;
|
|
var oldImpulse = this.m_angularImpulse;
|
|
var maxImpulse = h * this.m_maxTorque;
|
|
this.m_angularImpulse = math.clamp(this.m_angularImpulse + impulse, -maxImpulse, maxImpulse);
|
|
impulse = this.m_angularImpulse - oldImpulse;
|
|
wA -= iA * impulse;
|
|
wB += iB * impulse;
|
|
}
|
|
// Solve linear friction
|
|
{
|
|
var Cdot = Vec2.zero();
|
|
Cdot.addCombine(1, vB, 1, Vec2.cross(wB, this.m_rB));
|
|
Cdot.subCombine(1, vA, 1, Vec2.cross(wA, this.m_rA));
|
|
Cdot.addMul(inv_h * this.m_correctionFactor, this.m_linearError);
|
|
var impulse = Vec2.neg(Mat22.mulVec2(this.m_linearMass, Cdot));
|
|
var oldImpulse = Vec2.clone(this.m_linearImpulse);
|
|
this.m_linearImpulse.add(impulse);
|
|
var maxImpulse = h * this.m_maxForce;
|
|
this.m_linearImpulse.clamp(maxImpulse);
|
|
impulse = Vec2.sub(this.m_linearImpulse, oldImpulse);
|
|
vA.subMul(mA, impulse);
|
|
wA -= iA * Vec2.cross(this.m_rA, impulse);
|
|
vB.addMul(mB, impulse);
|
|
wB += iB * Vec2.cross(this.m_rB, impulse);
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
MotorJoint.prototype.solvePositionConstraints = function (step) {
|
|
return true;
|
|
};
|
|
MotorJoint.TYPE = 'motor-joint';
|
|
return MotorJoint;
|
|
}(Joint));
|
|
Joint.TYPES[MotorJoint.TYPE] = MotorJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$4 = {
|
|
maxForce: 0.0,
|
|
frequencyHz: 5.0,
|
|
dampingRatio: 0.7
|
|
};
|
|
/**
|
|
* A mouse joint is used to make a point on a body track a specified world
|
|
* point. This a soft constraint with a maximum force. This allows the
|
|
* constraint to stretch and without applying huge forces.
|
|
*
|
|
* NOTE: this joint is not documented in the manual because it was developed to
|
|
* be used in the testbed. If you want to learn how to use the mouse joint, look
|
|
* at the testbed.
|
|
*/
|
|
var MouseJoint = /** @class */ (function (_super) {
|
|
__extends(MouseJoint, _super);
|
|
function MouseJoint(def, bodyA, bodyB, target) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof MouseJoint)) {
|
|
return new MouseJoint(def, bodyA, bodyB, target);
|
|
}
|
|
def = options(def, DEFAULTS$4);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = MouseJoint.TYPE;
|
|
_this.m_targetA = target ? Vec2.clone(target) : def.target || Vec2.zero();
|
|
_this.m_localAnchorB = Transform.mulTVec2(bodyB.getTransform(), _this.m_targetA);
|
|
_this.m_maxForce = def.maxForce;
|
|
_this.m_impulse = Vec2.zero();
|
|
_this.m_frequencyHz = def.frequencyHz;
|
|
_this.m_dampingRatio = def.dampingRatio;
|
|
_this.m_beta = 0.0;
|
|
_this.m_gamma = 0.0;
|
|
// Solver temp
|
|
_this.m_rB = Vec2.zero();
|
|
_this.m_localCenterB = Vec2.zero();
|
|
_this.m_invMassB = 0.0;
|
|
_this.m_invIB = 0.0;
|
|
_this.m_mass = new Mat22();
|
|
_this.m_C = Vec2.zero();
|
|
return _this;
|
|
// p = attached point, m = mouse point
|
|
// C = p - m
|
|
// Cdot = v
|
|
// = v + cross(w, r)
|
|
// J = [I r_skew]
|
|
// Identity used:
|
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
|
}
|
|
/** @internal */
|
|
MouseJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
target: this.m_targetA,
|
|
maxForce: this.m_maxForce,
|
|
frequencyHz: this.m_frequencyHz,
|
|
dampingRatio: this.m_dampingRatio,
|
|
_localAnchorB: this.m_localAnchorB,
|
|
};
|
|
};
|
|
/** @internal */
|
|
MouseJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
data.target = new Vec2(data.target);
|
|
var joint = new MouseJoint(data);
|
|
if (data._localAnchorB) {
|
|
joint.m_localAnchorB = data._localAnchorB;
|
|
}
|
|
return joint;
|
|
};
|
|
/**
|
|
* Use this to update the target point.
|
|
*/
|
|
MouseJoint.prototype.setTarget = function (target) {
|
|
if (this.m_bodyB.isAwake() == false) {
|
|
this.m_bodyB.setAwake(true);
|
|
}
|
|
this.m_targetA = Vec2.clone(target);
|
|
};
|
|
MouseJoint.prototype.getTarget = function () {
|
|
return this.m_targetA;
|
|
};
|
|
/**
|
|
* Set the maximum force in Newtons.
|
|
*/
|
|
MouseJoint.prototype.setMaxForce = function (force) {
|
|
this.m_maxForce = force;
|
|
};
|
|
/**
|
|
* Get the maximum force in Newtons.
|
|
*/
|
|
MouseJoint.prototype.getMaxForce = function () {
|
|
return this.m_maxForce;
|
|
};
|
|
/**
|
|
* Set the frequency in Hertz.
|
|
*/
|
|
MouseJoint.prototype.setFrequency = function (hz) {
|
|
this.m_frequencyHz = hz;
|
|
};
|
|
/**
|
|
* Get the frequency in Hertz.
|
|
*/
|
|
MouseJoint.prototype.getFrequency = function () {
|
|
return this.m_frequencyHz;
|
|
};
|
|
/**
|
|
* Set the damping ratio (dimensionless).
|
|
*/
|
|
MouseJoint.prototype.setDampingRatio = function (ratio) {
|
|
this.m_dampingRatio = ratio;
|
|
};
|
|
/**
|
|
* Get the damping ratio (dimensionless).
|
|
*/
|
|
MouseJoint.prototype.getDampingRatio = function () {
|
|
return this.m_dampingRatio;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
MouseJoint.prototype.getAnchorA = function () {
|
|
return Vec2.clone(this.m_targetA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
MouseJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
MouseJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(inv_dt, this.m_impulse);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
MouseJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * 0.0;
|
|
};
|
|
/**
|
|
* Shift the origin for any points stored in world coordinates.
|
|
*/
|
|
MouseJoint.prototype.shiftOrigin = function (newOrigin) {
|
|
this.m_targetA.sub(newOrigin);
|
|
};
|
|
MouseJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var position = this.m_bodyB.c_position;
|
|
var velocity = this.m_bodyB.c_velocity;
|
|
var cB = position.c;
|
|
var aB = position.a;
|
|
var vB = velocity.v;
|
|
var wB = velocity.w;
|
|
var qB = Rot.neo(aB);
|
|
var mass = this.m_bodyB.getMass();
|
|
// Frequency
|
|
var omega = 2.0 * math.PI * this.m_frequencyHz;
|
|
// Damping coefficient
|
|
var d = 2.0 * mass * this.m_dampingRatio * omega;
|
|
// Spring stiffness
|
|
var k = mass * (omega * omega);
|
|
// magic formulas
|
|
// gamma has units of inverse mass.
|
|
// beta has units of inverse time.
|
|
var h = step.dt;
|
|
this.m_gamma = h * (d + h * k);
|
|
if (this.m_gamma != 0.0) {
|
|
this.m_gamma = 1.0 / this.m_gamma;
|
|
}
|
|
this.m_beta = h * k * this.m_gamma;
|
|
// Compute the effective mass matrix.
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) *
|
|
// invI2 * skew(r2)]
|
|
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y
|
|
// -r1.x*r1.y]
|
|
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
|
|
var K = new Mat22();
|
|
K.ex.x = this.m_invMassB + this.m_invIB * this.m_rB.y * this.m_rB.y
|
|
+ this.m_gamma;
|
|
K.ex.y = -this.m_invIB * this.m_rB.x * this.m_rB.y;
|
|
K.ey.x = K.ex.y;
|
|
K.ey.y = this.m_invMassB + this.m_invIB * this.m_rB.x * this.m_rB.x
|
|
+ this.m_gamma;
|
|
this.m_mass = K.getInverse();
|
|
this.m_C.set(cB);
|
|
this.m_C.addCombine(1, this.m_rB, -1, this.m_targetA);
|
|
this.m_C.mul(this.m_beta);
|
|
// Cheat with some damping
|
|
wB *= 0.98;
|
|
if (step.warmStarting) {
|
|
this.m_impulse.mul(step.dtRatio);
|
|
vB.addMul(this.m_invMassB, this.m_impulse);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, this.m_impulse);
|
|
}
|
|
else {
|
|
this.m_impulse.setZero();
|
|
}
|
|
velocity.v.set(vB);
|
|
velocity.w = wB;
|
|
};
|
|
MouseJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var velocity = this.m_bodyB.c_velocity;
|
|
var vB = Vec2.clone(velocity.v);
|
|
var wB = velocity.w;
|
|
// Cdot = v + cross(w, r)
|
|
var Cdot = Vec2.cross(wB, this.m_rB);
|
|
Cdot.add(vB);
|
|
Cdot.addCombine(1, this.m_C, this.m_gamma, this.m_impulse);
|
|
Cdot.neg();
|
|
var impulse = Mat22.mulVec2(this.m_mass, Cdot);
|
|
var oldImpulse = Vec2.clone(this.m_impulse);
|
|
this.m_impulse.add(impulse);
|
|
var maxImpulse = step.dt * this.m_maxForce;
|
|
this.m_impulse.clamp(maxImpulse);
|
|
impulse = Vec2.sub(this.m_impulse, oldImpulse);
|
|
vB.addMul(this.m_invMassB, impulse);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, impulse);
|
|
velocity.v.set(vB);
|
|
velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
MouseJoint.prototype.solvePositionConstraints = function (step) {
|
|
return true;
|
|
};
|
|
MouseJoint.TYPE = 'mouse-joint';
|
|
return MouseJoint;
|
|
}(Joint));
|
|
Joint.TYPES[MouseJoint.TYPE] = MouseJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$3 = {
|
|
collideConnected: true
|
|
};
|
|
/**
|
|
* The pulley joint is connected to two bodies and two fixed ground points. The
|
|
* pulley supports a ratio such that: length1 + ratio * length2 <= constant
|
|
*
|
|
* Yes, the force transmitted is scaled by the ratio.
|
|
*
|
|
* Warning: the pulley joint can get a bit squirrelly by itself. They often work
|
|
* better when combined with prismatic joints. You should also cover the the
|
|
* anchor points with static shapes to prevent one side from going to zero
|
|
* length.
|
|
*/
|
|
var PulleyJoint = /** @class */ (function (_super) {
|
|
__extends(PulleyJoint, _super);
|
|
function PulleyJoint(def, bodyA, bodyB, groundA, groundB, anchorA, anchorB, ratio) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof PulleyJoint)) {
|
|
return new PulleyJoint(def, bodyA, bodyB, groundA, groundB, anchorA, anchorB, ratio);
|
|
}
|
|
def = options(def, DEFAULTS$3);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = PulleyJoint.TYPE;
|
|
_this.m_groundAnchorA = groundA ? groundA : def.groundAnchorA || Vec2.neo(-1.0, 1.0);
|
|
_this.m_groundAnchorB = groundB ? groundB : def.groundAnchorB || Vec2.neo(1.0, 1.0);
|
|
_this.m_localAnchorA = anchorA ? bodyA.getLocalPoint(anchorA) : def.localAnchorA || Vec2.neo(-1.0, 0.0);
|
|
_this.m_localAnchorB = anchorB ? bodyB.getLocalPoint(anchorB) : def.localAnchorB || Vec2.neo(1.0, 0.0);
|
|
_this.m_lengthA = math.isFinite(def.lengthA) ? def.lengthA : Vec2.distance(anchorA, groundA);
|
|
_this.m_lengthB = math.isFinite(def.lengthB) ? def.lengthB : Vec2.distance(anchorB, groundB);
|
|
_this.m_ratio = math.isFinite(ratio) ? ratio : def.ratio;
|
|
_this.m_constant = _this.m_lengthA + _this.m_ratio * _this.m_lengthB;
|
|
_this.m_impulse = 0.0;
|
|
// Solver temp
|
|
_this.m_uA; // Vec2
|
|
_this.m_uB; // Vec2
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_mass; // float
|
|
return _this;
|
|
// Pulley:
|
|
// length1 = norm(p1 - s1)
|
|
// length2 = norm(p2 - s2)
|
|
// C0 = (length1 + ratio * length2)_initial
|
|
// C = C0 - (length1 + ratio * length2)
|
|
// u1 = (p1 - s1) / norm(p1 - s1)
|
|
// u2 = (p2 - s2) / norm(p2 - s2)
|
|
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
|
|
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
|
|
// K = J * invM * JT
|
|
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 *
|
|
// cross(r2, u2)^2)
|
|
}
|
|
PulleyJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
groundAnchorA: this.m_groundAnchorA,
|
|
groundAnchorB: this.m_groundAnchorB,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
lengthA: this.m_lengthA,
|
|
lengthB: this.m_lengthB,
|
|
ratio: this.m_ratio,
|
|
};
|
|
};
|
|
PulleyJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new PulleyJoint(data);
|
|
return joint;
|
|
};
|
|
/**
|
|
* Get the first ground anchor.
|
|
*/
|
|
PulleyJoint.prototype.getGroundAnchorA = function () {
|
|
return this.m_groundAnchorA;
|
|
};
|
|
/**
|
|
* Get the second ground anchor.
|
|
*/
|
|
PulleyJoint.prototype.getGroundAnchorB = function () {
|
|
return this.m_groundAnchorB;
|
|
};
|
|
/**
|
|
* Get the current length of the segment attached to bodyA.
|
|
*/
|
|
PulleyJoint.prototype.getLengthA = function () {
|
|
return this.m_lengthA;
|
|
};
|
|
/**
|
|
* Get the current length of the segment attached to bodyB.
|
|
*/
|
|
PulleyJoint.prototype.getLengthB = function () {
|
|
return this.m_lengthB;
|
|
};
|
|
/**
|
|
* Get the pulley ratio.
|
|
*/
|
|
PulleyJoint.prototype.getRatio = function () {
|
|
return this.m_ratio;
|
|
};
|
|
/**
|
|
* Get the current length of the segment attached to bodyA.
|
|
*/
|
|
PulleyJoint.prototype.getCurrentLengthA = function () {
|
|
var p = this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
var s = this.m_groundAnchorA;
|
|
return Vec2.distance(p, s);
|
|
};
|
|
/**
|
|
* Get the current length of the segment attached to bodyB.
|
|
*/
|
|
PulleyJoint.prototype.getCurrentLengthB = function () {
|
|
var p = this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
var s = this.m_groundAnchorB;
|
|
return Vec2.distance(p, s);
|
|
};
|
|
/**
|
|
* Shift the origin for any points stored in world coordinates.
|
|
*
|
|
* @param newOrigin
|
|
*/
|
|
PulleyJoint.prototype.shiftOrigin = function (newOrigin) {
|
|
this.m_groundAnchorA.sub(newOrigin);
|
|
this.m_groundAnchorB.sub(newOrigin);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
PulleyJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
PulleyJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
PulleyJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(this.m_impulse, this.m_uB).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
PulleyJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return 0.0;
|
|
};
|
|
PulleyJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// Get the pulley axes.
|
|
this.m_uA = Vec2.sub(Vec2.add(cA, this.m_rA), this.m_groundAnchorA);
|
|
this.m_uB = Vec2.sub(Vec2.add(cB, this.m_rB), this.m_groundAnchorB);
|
|
var lengthA = this.m_uA.length();
|
|
var lengthB = this.m_uB.length();
|
|
if (lengthA > 10.0 * Settings.linearSlop) {
|
|
this.m_uA.mul(1.0 / lengthA);
|
|
}
|
|
else {
|
|
this.m_uA.setZero();
|
|
}
|
|
if (lengthB > 10.0 * Settings.linearSlop) {
|
|
this.m_uB.mul(1.0 / lengthB);
|
|
}
|
|
else {
|
|
this.m_uB.setZero();
|
|
}
|
|
// Compute effective mass.
|
|
var ruA = Vec2.cross(this.m_rA, this.m_uA); // float
|
|
var ruB = Vec2.cross(this.m_rB, this.m_uB); // float
|
|
var mA = this.m_invMassA + this.m_invIA * ruA * ruA; // float
|
|
var mB = this.m_invMassB + this.m_invIB * ruB * ruB; // float
|
|
this.m_mass = mA + this.m_ratio * this.m_ratio * mB;
|
|
if (this.m_mass > 0.0) {
|
|
this.m_mass = 1.0 / this.m_mass;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Scale impulses to support variable time steps.
|
|
this.m_impulse *= step.dtRatio;
|
|
// Warm starting.
|
|
var PA = Vec2.mul(-this.m_impulse, this.m_uA);
|
|
var PB = Vec2.mul(-this.m_ratio * this.m_impulse, this.m_uB);
|
|
vA.addMul(this.m_invMassA, PA);
|
|
wA += this.m_invIA * Vec2.cross(this.m_rA, PA);
|
|
vB.addMul(this.m_invMassB, PB);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, PB);
|
|
}
|
|
else {
|
|
this.m_impulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
PulleyJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var vpA = Vec2.add(vA, Vec2.cross(wA, this.m_rA));
|
|
var vpB = Vec2.add(vB, Vec2.cross(wB, this.m_rB));
|
|
var Cdot = -Vec2.dot(this.m_uA, vpA) - this.m_ratio
|
|
* Vec2.dot(this.m_uB, vpB); // float
|
|
var impulse = -this.m_mass * Cdot; // float
|
|
this.m_impulse += impulse;
|
|
var PA = Vec2.mul(-impulse, this.m_uA); // Vec2
|
|
var PB = Vec2.mul(-this.m_ratio * impulse, this.m_uB); // Vec2
|
|
vA.addMul(this.m_invMassA, PA);
|
|
wA += this.m_invIA * Vec2.cross(this.m_rA, PA);
|
|
vB.addMul(this.m_invMassB, PB);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, PB);
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
PulleyJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA), qB = Rot.neo(aB);
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// Get the pulley axes.
|
|
var uA = Vec2.sub(Vec2.add(cA, this.m_rA), this.m_groundAnchorA);
|
|
var uB = Vec2.sub(Vec2.add(cB, this.m_rB), this.m_groundAnchorB);
|
|
var lengthA = uA.length();
|
|
var lengthB = uB.length();
|
|
if (lengthA > 10.0 * Settings.linearSlop) {
|
|
uA.mul(1.0 / lengthA);
|
|
}
|
|
else {
|
|
uA.setZero();
|
|
}
|
|
if (lengthB > 10.0 * Settings.linearSlop) {
|
|
uB.mul(1.0 / lengthB);
|
|
}
|
|
else {
|
|
uB.setZero();
|
|
}
|
|
// Compute effective mass.
|
|
var ruA = Vec2.cross(rA, uA);
|
|
var ruB = Vec2.cross(rB, uB);
|
|
var mA = this.m_invMassA + this.m_invIA * ruA * ruA; // float
|
|
var mB = this.m_invMassB + this.m_invIB * ruB * ruB; // float
|
|
var mass = mA + this.m_ratio * this.m_ratio * mB; // float
|
|
if (mass > 0.0) {
|
|
mass = 1.0 / mass;
|
|
}
|
|
var C = this.m_constant - lengthA - this.m_ratio * lengthB; // float
|
|
var linearError = math.abs(C); // float
|
|
var impulse = -mass * C; // float
|
|
var PA = Vec2.mul(-impulse, uA); // Vec2
|
|
var PB = Vec2.mul(-this.m_ratio * impulse, uB); // Vec2
|
|
cA.addMul(this.m_invMassA, PA);
|
|
aA += this.m_invIA * Vec2.cross(rA, PA);
|
|
cB.addMul(this.m_invMassB, PB);
|
|
aB += this.m_invIB * Vec2.cross(rB, PB);
|
|
this.m_bodyA.c_position.c = cA;
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c = cB;
|
|
this.m_bodyB.c_position.a = aB;
|
|
return linearError < Settings.linearSlop;
|
|
};
|
|
PulleyJoint.TYPE = 'pulley-joint';
|
|
PulleyJoint.MIN_PULLEY_LENGTH = 2.0; // minPulleyLength
|
|
return PulleyJoint;
|
|
}(Joint));
|
|
Joint.TYPES[PulleyJoint.TYPE] = PulleyJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var inactiveLimit = 0;
|
|
var atUpperLimit = 2;
|
|
var DEFAULTS$2 = {
|
|
maxLength: 0.0,
|
|
};
|
|
/**
|
|
* A rope joint enforces a maximum distance between two points on two bodies. It
|
|
* has no other effect.
|
|
*
|
|
* Warning: if you attempt to change the maximum length during the simulation
|
|
* you will get some non-physical behavior.
|
|
*
|
|
* A model that would allow you to dynamically modify the length would have some
|
|
* sponginess, so I chose not to implement it that way. See DistanceJoint if you
|
|
* want to dynamically control length.
|
|
*/
|
|
var RopeJoint = /** @class */ (function (_super) {
|
|
__extends(RopeJoint, _super);
|
|
function RopeJoint(def, bodyA, bodyB, anchor) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof RopeJoint)) {
|
|
return new RopeJoint(def, bodyA, bodyB, anchor);
|
|
}
|
|
def = options(def, DEFAULTS$2);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = RopeJoint.TYPE;
|
|
_this.m_localAnchorA = anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.neo(-1.0, 0.0);
|
|
_this.m_localAnchorB = anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.neo(1.0, 0.0);
|
|
_this.m_maxLength = def.maxLength;
|
|
_this.m_mass = 0.0;
|
|
_this.m_impulse = 0.0;
|
|
_this.m_length = 0.0;
|
|
_this.m_state = inactiveLimit;
|
|
// Solver temp
|
|
_this.m_u; // Vec2
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_mass; // float
|
|
return _this;
|
|
// Limit:
|
|
// C = norm(pB - pA) - L
|
|
// u = (pB - pA) / norm(pB - pA)
|
|
// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
|
|
// J = [-u -cross(rA, u) u cross(rB, u)]
|
|
// K = J * invM * JT
|
|
// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
|
|
}
|
|
/** @internal */
|
|
RopeJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
maxLength: this.m_maxLength,
|
|
};
|
|
};
|
|
/** @internal */
|
|
RopeJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new RopeJoint(data);
|
|
return joint;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
RopeJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
RopeJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* Set the maximum length of the rope.
|
|
*/
|
|
RopeJoint.prototype.setMaxLength = function (length) {
|
|
this.m_maxLength = length;
|
|
};
|
|
/**
|
|
* Get the maximum length of the rope.
|
|
*/
|
|
RopeJoint.prototype.getMaxLength = function () {
|
|
return this.m_maxLength;
|
|
};
|
|
RopeJoint.prototype.getLimitState = function () {
|
|
// TODO LimitState
|
|
return this.m_state;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
RopeJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
RopeJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
RopeJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.mul(this.m_impulse, this.m_u).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
RopeJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return 0.0;
|
|
};
|
|
RopeJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
this.m_rA = Rot.mulSub(qA, this.m_localAnchorA, this.m_localCenterA);
|
|
this.m_rB = Rot.mulSub(qB, this.m_localAnchorB, this.m_localCenterB);
|
|
this.m_u = Vec2.zero();
|
|
this.m_u.addCombine(1, cB, 1, this.m_rB);
|
|
this.m_u.subCombine(1, cA, 1, this.m_rA); // Vec2
|
|
this.m_length = this.m_u.length();
|
|
var C = this.m_length - this.m_maxLength; // float
|
|
if (C > 0.0) {
|
|
this.m_state = atUpperLimit;
|
|
}
|
|
else {
|
|
this.m_state = inactiveLimit;
|
|
}
|
|
if (this.m_length > Settings.linearSlop) {
|
|
this.m_u.mul(1.0 / this.m_length);
|
|
}
|
|
else {
|
|
this.m_u.setZero();
|
|
this.m_mass = 0.0;
|
|
this.m_impulse = 0.0;
|
|
return;
|
|
}
|
|
// Compute effective mass.
|
|
var crA = Vec2.cross(this.m_rA, this.m_u); // float
|
|
var crB = Vec2.cross(this.m_rB, this.m_u); // float
|
|
var invMass = this.m_invMassA + this.m_invIA * crA * crA + this.m_invMassB
|
|
+ this.m_invIB * crB * crB; // float
|
|
this.m_mass = invMass != 0.0 ? 1.0 / invMass : 0.0;
|
|
if (step.warmStarting) {
|
|
// Scale the impulse to support a variable time step.
|
|
this.m_impulse *= step.dtRatio;
|
|
var P = Vec2.mul(this.m_impulse, this.m_u);
|
|
vA.subMul(this.m_invMassA, P);
|
|
wA -= this.m_invIA * Vec2.cross(this.m_rA, P);
|
|
vB.addMul(this.m_invMassB, P);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, P);
|
|
}
|
|
else {
|
|
this.m_impulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
RopeJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
// Cdot = dot(u, v + cross(w, r))
|
|
var vpA = Vec2.addCross(vA, wA, this.m_rA); // Vec2
|
|
var vpB = Vec2.addCross(vB, wB, this.m_rB); // Vec2
|
|
var C = this.m_length - this.m_maxLength; // float
|
|
var Cdot = Vec2.dot(this.m_u, Vec2.sub(vpB, vpA)); // float
|
|
// Predictive constraint.
|
|
if (C < 0.0) {
|
|
Cdot += step.inv_dt * C;
|
|
}
|
|
var impulse = -this.m_mass * Cdot; // float
|
|
var oldImpulse = this.m_impulse; // float
|
|
this.m_impulse = math.min(0.0, this.m_impulse + impulse);
|
|
impulse = this.m_impulse - oldImpulse;
|
|
var P = Vec2.mul(impulse, this.m_u); // Vec2
|
|
vA.subMul(this.m_invMassA, P);
|
|
wA -= this.m_invIA * Vec2.cross(this.m_rA, P);
|
|
vB.addMul(this.m_invMassB, P);
|
|
wB += this.m_invIB * Vec2.cross(this.m_rB, P);
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
RopeJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c; // Vec2
|
|
var aA = this.m_bodyA.c_position.a; // float
|
|
var cB = this.m_bodyB.c_position.c; // Vec2
|
|
var aB = this.m_bodyB.c_position.a; // float
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var rA = Rot.mulSub(qA, this.m_localAnchorA, this.m_localCenterA);
|
|
var rB = Rot.mulSub(qB, this.m_localAnchorB, this.m_localCenterB);
|
|
var u = Vec2.zero();
|
|
u.addCombine(1, cB, 1, rB);
|
|
u.subCombine(1, cA, 1, rA); // Vec2
|
|
var length = u.normalize(); // float
|
|
var C = length - this.m_maxLength; // float
|
|
C = math.clamp(C, 0.0, Settings.maxLinearCorrection);
|
|
var impulse = -this.m_mass * C; // float
|
|
var P = Vec2.mul(impulse, u); // Vec2
|
|
cA.subMul(this.m_invMassA, P);
|
|
aA -= this.m_invIA * Vec2.cross(rA, P);
|
|
cB.addMul(this.m_invMassB, P);
|
|
aB += this.m_invIB * Vec2.cross(rB, P);
|
|
this.m_bodyA.c_position.c.set(cA);
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c.set(cB);
|
|
this.m_bodyB.c_position.a = aB;
|
|
return length - this.m_maxLength < Settings.linearSlop;
|
|
};
|
|
RopeJoint.TYPE = 'rope-joint';
|
|
return RopeJoint;
|
|
}(Joint));
|
|
Joint.TYPES[RopeJoint.TYPE] = RopeJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS$1 = {
|
|
frequencyHz: 0.0,
|
|
dampingRatio: 0.0,
|
|
};
|
|
/**
|
|
* A weld joint essentially glues two bodies together. A weld joint may distort
|
|
* somewhat because the island constraint solver is approximate.
|
|
*/
|
|
var WeldJoint = /** @class */ (function (_super) {
|
|
__extends(WeldJoint, _super);
|
|
function WeldJoint(def, bodyA, bodyB, anchor) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof WeldJoint)) {
|
|
return new WeldJoint(def, bodyA, bodyB, anchor);
|
|
}
|
|
def = options(def, DEFAULTS$1);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = WeldJoint.TYPE;
|
|
_this.m_localAnchorA = Vec2.clone(anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.zero());
|
|
_this.m_referenceAngle = math.isFinite(def.referenceAngle) ? def.referenceAngle : bodyB.getAngle() - bodyA.getAngle();
|
|
_this.m_frequencyHz = def.frequencyHz;
|
|
_this.m_dampingRatio = def.dampingRatio;
|
|
_this.m_impulse = new Vec3();
|
|
_this.m_bias = 0.0;
|
|
_this.m_gamma = 0.0;
|
|
// Solver temp
|
|
_this.m_rA; // Vec2
|
|
_this.m_rB; // Vec2
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_mass = new Mat33();
|
|
return _this;
|
|
// Point-to-point constraint
|
|
// C = p2 - p1
|
|
// Cdot = v2 - v1
|
|
// / = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
|
// J = [-I -r1_skew I r2_skew ]
|
|
// Identity used:
|
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
|
// Angle constraint
|
|
// C = angle2 - angle1 - referenceAngle
|
|
// Cdot = w2 - w1
|
|
// J = [0 0 -1 0 0 1]
|
|
// K = invI1 + invI2
|
|
}
|
|
/** @internal */
|
|
WeldJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
frequencyHz: this.m_frequencyHz,
|
|
dampingRatio: this.m_dampingRatio,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
referenceAngle: this.m_referenceAngle,
|
|
};
|
|
};
|
|
/** @internal */
|
|
WeldJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new WeldJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
WeldJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
WeldJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
WeldJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* Get the reference angle.
|
|
*/
|
|
WeldJoint.prototype.getReferenceAngle = function () {
|
|
return this.m_referenceAngle;
|
|
};
|
|
/**
|
|
* Set frequency in Hz.
|
|
*/
|
|
WeldJoint.prototype.setFrequency = function (hz) {
|
|
this.m_frequencyHz = hz;
|
|
};
|
|
/**
|
|
* Get frequency in Hz.
|
|
*/
|
|
WeldJoint.prototype.getFrequency = function () {
|
|
return this.m_frequencyHz;
|
|
};
|
|
/**
|
|
* Set damping ratio.
|
|
*/
|
|
WeldJoint.prototype.setDampingRatio = function (ratio) {
|
|
this.m_dampingRatio = ratio;
|
|
};
|
|
/**
|
|
* Get damping ratio.
|
|
*/
|
|
WeldJoint.prototype.getDampingRatio = function () {
|
|
return this.m_dampingRatio;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
WeldJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
WeldJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
WeldJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.neo(this.m_impulse.x, this.m_impulse.y).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
WeldJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_impulse.z;
|
|
};
|
|
WeldJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA), qB = Rot.neo(aB);
|
|
this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
this.m_rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
// J = [-I -r1_skew I r2_skew]
|
|
// [ 0 -1 0 1]
|
|
// r_skew = [-ry; rx]
|
|
// Matlab
|
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var K = new Mat33();
|
|
K.ex.x = mA + mB + this.m_rA.y * this.m_rA.y * iA + this.m_rB.y * this.m_rB.y
|
|
* iB;
|
|
K.ey.x = -this.m_rA.y * this.m_rA.x * iA - this.m_rB.y * this.m_rB.x * iB;
|
|
K.ez.x = -this.m_rA.y * iA - this.m_rB.y * iB;
|
|
K.ex.y = K.ey.x;
|
|
K.ey.y = mA + mB + this.m_rA.x * this.m_rA.x * iA + this.m_rB.x * this.m_rB.x
|
|
* iB;
|
|
K.ez.y = this.m_rA.x * iA + this.m_rB.x * iB;
|
|
K.ex.z = K.ez.x;
|
|
K.ey.z = K.ez.y;
|
|
K.ez.z = iA + iB;
|
|
if (this.m_frequencyHz > 0.0) {
|
|
K.getInverse22(this.m_mass);
|
|
var invM = iA + iB; // float
|
|
var m = invM > 0.0 ? 1.0 / invM : 0.0; // float
|
|
var C = aB - aA - this.m_referenceAngle; // float
|
|
// Frequency
|
|
var omega = 2.0 * math.PI * this.m_frequencyHz; // float
|
|
// Damping coefficient
|
|
var d = 2.0 * m * this.m_dampingRatio * omega; // float
|
|
// Spring stiffness
|
|
var k = m * omega * omega; // float
|
|
// magic formulas
|
|
var h = step.dt; // float
|
|
this.m_gamma = h * (d + h * k);
|
|
this.m_gamma = this.m_gamma != 0.0 ? 1.0 / this.m_gamma : 0.0;
|
|
this.m_bias = C * h * k * this.m_gamma;
|
|
invM += this.m_gamma;
|
|
this.m_mass.ez.z = invM != 0.0 ? 1.0 / invM : 0.0;
|
|
}
|
|
else if (K.ez.z == 0.0) {
|
|
K.getInverse22(this.m_mass);
|
|
this.m_gamma = 0.0;
|
|
this.m_bias = 0.0;
|
|
}
|
|
else {
|
|
K.getSymInverse33(this.m_mass);
|
|
this.m_gamma = 0.0;
|
|
this.m_bias = 0.0;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Scale impulses to support a variable time step.
|
|
this.m_impulse.mul(step.dtRatio);
|
|
var P = Vec2.neo(this.m_impulse.x, this.m_impulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + this.m_impulse.z);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + this.m_impulse.z);
|
|
}
|
|
else {
|
|
this.m_impulse.setZero();
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
WeldJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
if (this.m_frequencyHz > 0.0) {
|
|
var Cdot2 = wB - wA; // float
|
|
var impulse2 = -this.m_mass.ez.z
|
|
* (Cdot2 + this.m_bias + this.m_gamma * this.m_impulse.z); // float
|
|
this.m_impulse.z += impulse2;
|
|
wA -= iA * impulse2;
|
|
wB += iB * impulse2;
|
|
var Cdot1 = Vec2.zero();
|
|
Cdot1.addCombine(1, vB, 1, Vec2.cross(wB, this.m_rB));
|
|
Cdot1.subCombine(1, vA, 1, Vec2.cross(wA, this.m_rA)); // Vec2
|
|
var impulse1 = Vec2.neg(Mat33.mulVec2(this.m_mass, Cdot1)); // Vec2
|
|
this.m_impulse.x += impulse1.x;
|
|
this.m_impulse.y += impulse1.y;
|
|
var P = Vec2.clone(impulse1); // Vec2
|
|
vA.subMul(mA, P);
|
|
wA -= iA * Vec2.cross(this.m_rA, P);
|
|
vB.addMul(mB, P);
|
|
wB += iB * Vec2.cross(this.m_rB, P);
|
|
}
|
|
else {
|
|
var Cdot1 = Vec2.zero();
|
|
Cdot1.addCombine(1, vB, 1, Vec2.cross(wB, this.m_rB));
|
|
Cdot1.subCombine(1, vA, 1, Vec2.cross(wA, this.m_rA)); // Vec2
|
|
var Cdot2 = wB - wA; // float
|
|
var Cdot = new Vec3(Cdot1.x, Cdot1.y, Cdot2); // Vec3
|
|
var impulse = Vec3.neg(Mat33.mulVec3(this.m_mass, Cdot)); // Vec3
|
|
this.m_impulse.add(impulse);
|
|
var P = Vec2.neo(impulse.x, impulse.y);
|
|
vA.subMul(mA, P);
|
|
wA -= iA * (Vec2.cross(this.m_rA, P) + impulse.z);
|
|
vB.addMul(mB, P);
|
|
wB += iB * (Vec2.cross(this.m_rB, P) + impulse.z);
|
|
}
|
|
this.m_bodyA.c_velocity.v = vA;
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v = vB;
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
WeldJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA), qB = Rot.neo(aB);
|
|
var mA = this.m_invMassA, mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA, iB = this.m_invIB; // float
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
var positionError, angularError; // float
|
|
var K = new Mat33();
|
|
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
|
|
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
|
|
K.ez.x = -rA.y * iA - rB.y * iB;
|
|
K.ex.y = K.ey.x;
|
|
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
|
|
K.ez.y = rA.x * iA + rB.x * iB;
|
|
K.ex.z = K.ez.x;
|
|
K.ey.z = K.ez.y;
|
|
K.ez.z = iA + iB;
|
|
if (this.m_frequencyHz > 0.0) {
|
|
var C1 = Vec2.zero();
|
|
C1.addCombine(1, cB, 1, rB);
|
|
C1.subCombine(1, cA, 1, rA); // Vec2
|
|
positionError = C1.length();
|
|
angularError = 0.0;
|
|
var P = Vec2.neg(K.solve22(C1)); // Vec2
|
|
cA.subMul(mA, P);
|
|
aA -= iA * Vec2.cross(rA, P);
|
|
cB.addMul(mB, P);
|
|
aB += iB * Vec2.cross(rB, P);
|
|
}
|
|
else {
|
|
var C1 = Vec2.zero();
|
|
C1.addCombine(1, cB, 1, rB);
|
|
C1.subCombine(1, cA, 1, rA);
|
|
var C2 = aB - aA - this.m_referenceAngle; // float
|
|
positionError = C1.length();
|
|
angularError = math.abs(C2);
|
|
var C = new Vec3(C1.x, C1.y, C2);
|
|
var impulse = new Vec3();
|
|
if (K.ez.z > 0.0) {
|
|
impulse = Vec3.neg(K.solve33(C));
|
|
}
|
|
else {
|
|
var impulse2 = Vec2.neg(K.solve22(C1));
|
|
impulse.set(impulse2.x, impulse2.y, 0.0);
|
|
}
|
|
var P = Vec2.neo(impulse.x, impulse.y);
|
|
cA.subMul(mA, P);
|
|
aA -= iA * (Vec2.cross(rA, P) + impulse.z);
|
|
cB.addMul(mB, P);
|
|
aB += iB * (Vec2.cross(rB, P) + impulse.z);
|
|
}
|
|
this.m_bodyA.c_position.c = cA;
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c = cB;
|
|
this.m_bodyB.c_position.a = aB;
|
|
return positionError <= Settings.linearSlop
|
|
&& angularError <= Settings.angularSlop;
|
|
};
|
|
WeldJoint.TYPE = 'weld-joint';
|
|
return WeldJoint;
|
|
}(Joint));
|
|
Joint.TYPES[WeldJoint.TYPE] = WeldJoint;
|
|
|
|
/*
|
|
* Planck.js
|
|
* The MIT License
|
|
* Copyright (c) 2021 Erin Catto, Ali Shakiba
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
var DEFAULTS = {
|
|
enableMotor: false,
|
|
maxMotorTorque: 0.0,
|
|
motorSpeed: 0.0,
|
|
frequencyHz: 2.0,
|
|
dampingRatio: 0.7,
|
|
};
|
|
/**
|
|
* A wheel joint. This joint provides two degrees of freedom: translation along
|
|
* an axis fixed in bodyA and rotation in the plane. In other words, it is a
|
|
* point to line constraint with a rotational motor and a linear spring/damper.
|
|
* This joint is designed for vehicle suspensions.
|
|
*/
|
|
var WheelJoint = /** @class */ (function (_super) {
|
|
__extends(WheelJoint, _super);
|
|
function WheelJoint(def, bodyA, bodyB, anchor, axis) {
|
|
var _this = this;
|
|
// @ts-ignore
|
|
if (!(_this instanceof WheelJoint)) {
|
|
return new WheelJoint(def, bodyA, bodyB, anchor, axis);
|
|
}
|
|
def = options(def, DEFAULTS);
|
|
_this = _super.call(this, def, bodyA, bodyB) || this;
|
|
bodyA = _this.m_bodyA;
|
|
bodyB = _this.m_bodyB;
|
|
_this.m_type = WheelJoint.TYPE;
|
|
_this.m_localAnchorA = Vec2.clone(anchor ? bodyA.getLocalPoint(anchor) : def.localAnchorA || Vec2.zero());
|
|
_this.m_localAnchorB = Vec2.clone(anchor ? bodyB.getLocalPoint(anchor) : def.localAnchorB || Vec2.zero());
|
|
// @ts-ignore localAxis
|
|
_this.m_localXAxisA = Vec2.clone(axis ? bodyA.getLocalVector(axis) : def.localAxisA || def.localAxis || Vec2.neo(1.0, 0.0));
|
|
_this.m_localYAxisA = Vec2.cross(1.0, _this.m_localXAxisA);
|
|
_this.m_mass = 0.0;
|
|
_this.m_impulse = 0.0;
|
|
_this.m_motorMass = 0.0;
|
|
_this.m_motorImpulse = 0.0;
|
|
_this.m_springMass = 0.0;
|
|
_this.m_springImpulse = 0.0;
|
|
_this.m_maxMotorTorque = def.maxMotorTorque;
|
|
_this.m_motorSpeed = def.motorSpeed;
|
|
_this.m_enableMotor = def.enableMotor;
|
|
_this.m_frequencyHz = def.frequencyHz;
|
|
_this.m_dampingRatio = def.dampingRatio;
|
|
_this.m_bias = 0.0;
|
|
_this.m_gamma = 0.0;
|
|
// Solver temp
|
|
_this.m_localCenterA; // Vec2
|
|
_this.m_localCenterB; // Vec2
|
|
_this.m_invMassA; // float
|
|
_this.m_invMassB; // float
|
|
_this.m_invIA; // float
|
|
_this.m_invIB; // float
|
|
_this.m_ax = Vec2.zero();
|
|
_this.m_ay = Vec2.zero(); // Vec2
|
|
_this.m_sAx;
|
|
_this.m_sBx; // float
|
|
_this.m_sAy;
|
|
_this.m_sBy; // float
|
|
return _this;
|
|
// Linear constraint (point-to-line)
|
|
// d = pB - pA = xB + rB - xA - rA
|
|
// C = dot(ay, d)
|
|
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA,
|
|
// rA))
|
|
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB,
|
|
// ay), vB)
|
|
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
|
|
// Spring linear constraint
|
|
// C = dot(ax, d)
|
|
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) +
|
|
// dot(cross(rB, ax), vB)
|
|
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
|
|
// Motor rotational constraint
|
|
// Cdot = wB - wA
|
|
// J = [0 0 -1 0 0 1]
|
|
}
|
|
/** @internal */
|
|
WheelJoint.prototype._serialize = function () {
|
|
return {
|
|
type: this.m_type,
|
|
bodyA: this.m_bodyA,
|
|
bodyB: this.m_bodyB,
|
|
collideConnected: this.m_collideConnected,
|
|
enableMotor: this.m_enableMotor,
|
|
maxMotorTorque: this.m_maxMotorTorque,
|
|
motorSpeed: this.m_motorSpeed,
|
|
frequencyHz: this.m_frequencyHz,
|
|
dampingRatio: this.m_dampingRatio,
|
|
localAnchorA: this.m_localAnchorA,
|
|
localAnchorB: this.m_localAnchorB,
|
|
localAxisA: this.m_localXAxisA,
|
|
};
|
|
};
|
|
/** @internal */
|
|
WheelJoint._deserialize = function (data, world, restore) {
|
|
data = __assign({}, data);
|
|
data.bodyA = restore(Body, data.bodyA, world);
|
|
data.bodyB = restore(Body, data.bodyB, world);
|
|
var joint = new WheelJoint(data);
|
|
return joint;
|
|
};
|
|
/** @internal */
|
|
WheelJoint.prototype._setAnchors = function (def) {
|
|
if (def.anchorA) {
|
|
this.m_localAnchorA.set(this.m_bodyA.getLocalPoint(def.anchorA));
|
|
}
|
|
else if (def.localAnchorA) {
|
|
this.m_localAnchorA.set(def.localAnchorA);
|
|
}
|
|
if (def.anchorB) {
|
|
this.m_localAnchorB.set(this.m_bodyB.getLocalPoint(def.anchorB));
|
|
}
|
|
else if (def.localAnchorB) {
|
|
this.m_localAnchorB.set(def.localAnchorB);
|
|
}
|
|
if (def.localAxisA) {
|
|
this.m_localXAxisA.set(def.localAxisA);
|
|
this.m_localYAxisA.set(Vec2.cross(1.0, def.localAxisA));
|
|
}
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyA's origin.
|
|
*/
|
|
WheelJoint.prototype.getLocalAnchorA = function () {
|
|
return this.m_localAnchorA;
|
|
};
|
|
/**
|
|
* The local anchor point relative to bodyB's origin.
|
|
*/
|
|
WheelJoint.prototype.getLocalAnchorB = function () {
|
|
return this.m_localAnchorB;
|
|
};
|
|
/**
|
|
* The local joint axis relative to bodyA.
|
|
*/
|
|
WheelJoint.prototype.getLocalAxisA = function () {
|
|
return this.m_localXAxisA;
|
|
};
|
|
/**
|
|
* Get the current joint translation, usually in meters.
|
|
*/
|
|
WheelJoint.prototype.getJointTranslation = function () {
|
|
var bA = this.m_bodyA;
|
|
var bB = this.m_bodyB;
|
|
var pA = bA.getWorldPoint(this.m_localAnchorA); // Vec2
|
|
var pB = bB.getWorldPoint(this.m_localAnchorB); // Vec2
|
|
var d = Vec2.sub(pB, pA); // Vec2
|
|
var axis = bA.getWorldVector(this.m_localXAxisA); // Vec2
|
|
var translation = Vec2.dot(d, axis); // float
|
|
return translation;
|
|
};
|
|
/**
|
|
* Get the current joint translation speed, usually in meters per second.
|
|
*/
|
|
WheelJoint.prototype.getJointSpeed = function () {
|
|
var wA = this.m_bodyA.m_angularVelocity;
|
|
var wB = this.m_bodyB.m_angularVelocity;
|
|
return wB - wA;
|
|
};
|
|
/**
|
|
* Is the joint motor enabled?
|
|
*/
|
|
WheelJoint.prototype.isMotorEnabled = function () {
|
|
return this.m_enableMotor;
|
|
};
|
|
/**
|
|
* Enable/disable the joint motor.
|
|
*/
|
|
WheelJoint.prototype.enableMotor = function (flag) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_enableMotor = flag;
|
|
};
|
|
/**
|
|
* Set the motor speed, usually in radians per second.
|
|
*/
|
|
WheelJoint.prototype.setMotorSpeed = function (speed) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_motorSpeed = speed;
|
|
};
|
|
/**
|
|
* Get the motor speed, usually in radians per second.
|
|
*/
|
|
WheelJoint.prototype.getMotorSpeed = function () {
|
|
return this.m_motorSpeed;
|
|
};
|
|
/**
|
|
* Set/Get the maximum motor force, usually in N-m.
|
|
*/
|
|
WheelJoint.prototype.setMaxMotorTorque = function (torque) {
|
|
this.m_bodyA.setAwake(true);
|
|
this.m_bodyB.setAwake(true);
|
|
this.m_maxMotorTorque = torque;
|
|
};
|
|
WheelJoint.prototype.getMaxMotorTorque = function () {
|
|
return this.m_maxMotorTorque;
|
|
};
|
|
/**
|
|
* Get the current motor torque given the inverse time step, usually in N-m.
|
|
*/
|
|
WheelJoint.prototype.getMotorTorque = function (inv_dt) {
|
|
return inv_dt * this.m_motorImpulse;
|
|
};
|
|
/**
|
|
* Set/Get the spring frequency in hertz. Setting the frequency to zero disables
|
|
* the spring.
|
|
*/
|
|
WheelJoint.prototype.setSpringFrequencyHz = function (hz) {
|
|
this.m_frequencyHz = hz;
|
|
};
|
|
WheelJoint.prototype.getSpringFrequencyHz = function () {
|
|
return this.m_frequencyHz;
|
|
};
|
|
/**
|
|
* Set/Get the spring damping ratio
|
|
*/
|
|
WheelJoint.prototype.setSpringDampingRatio = function (ratio) {
|
|
this.m_dampingRatio = ratio;
|
|
};
|
|
WheelJoint.prototype.getSpringDampingRatio = function () {
|
|
return this.m_dampingRatio;
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyA in world coordinates.
|
|
*/
|
|
WheelJoint.prototype.getAnchorA = function () {
|
|
return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
|
|
};
|
|
/**
|
|
* Get the anchor point on bodyB in world coordinates.
|
|
*/
|
|
WheelJoint.prototype.getAnchorB = function () {
|
|
return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
|
|
};
|
|
/**
|
|
* Get the reaction force on bodyB at the joint anchor in Newtons.
|
|
*/
|
|
WheelJoint.prototype.getReactionForce = function (inv_dt) {
|
|
return Vec2.combine(this.m_impulse, this.m_ay, this.m_springImpulse, this.m_ax).mul(inv_dt);
|
|
};
|
|
/**
|
|
* Get the reaction torque on bodyB in N*m.
|
|
*/
|
|
WheelJoint.prototype.getReactionTorque = function (inv_dt) {
|
|
return inv_dt * this.m_motorImpulse;
|
|
};
|
|
WheelJoint.prototype.initVelocityConstraints = function (step) {
|
|
this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
|
|
this.m_localCenterB = this.m_bodyB.m_sweep.localCenter;
|
|
this.m_invMassA = this.m_bodyA.m_invMass;
|
|
this.m_invMassB = this.m_bodyB.m_invMass;
|
|
this.m_invIA = this.m_bodyA.m_invI;
|
|
this.m_invIB = this.m_bodyB.m_invI;
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
// Compute the effective masses.
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
var d = Vec2.zero();
|
|
d.addCombine(1, cB, 1, rB);
|
|
d.subCombine(1, cA, 1, rA); // Vec2
|
|
// Point to line constraint
|
|
{
|
|
this.m_ay = Rot.mulVec2(qA, this.m_localYAxisA);
|
|
this.m_sAy = Vec2.cross(Vec2.add(d, rA), this.m_ay);
|
|
this.m_sBy = Vec2.cross(rB, this.m_ay);
|
|
this.m_mass = mA + mB + iA * this.m_sAy * this.m_sAy + iB * this.m_sBy
|
|
* this.m_sBy;
|
|
if (this.m_mass > 0.0) {
|
|
this.m_mass = 1.0 / this.m_mass;
|
|
}
|
|
}
|
|
// Spring constraint
|
|
this.m_springMass = 0.0;
|
|
this.m_bias = 0.0;
|
|
this.m_gamma = 0.0;
|
|
if (this.m_frequencyHz > 0.0) {
|
|
this.m_ax = Rot.mulVec2(qA, this.m_localXAxisA);
|
|
this.m_sAx = Vec2.cross(Vec2.add(d, rA), this.m_ax);
|
|
this.m_sBx = Vec2.cross(rB, this.m_ax);
|
|
var invMass = mA + mB + iA * this.m_sAx * this.m_sAx + iB * this.m_sBx
|
|
* this.m_sBx; // float
|
|
if (invMass > 0.0) {
|
|
this.m_springMass = 1.0 / invMass;
|
|
var C = Vec2.dot(d, this.m_ax); // float
|
|
// Frequency
|
|
var omega = 2.0 * math.PI * this.m_frequencyHz; // float
|
|
// Damping coefficient
|
|
var damp = 2.0 * this.m_springMass * this.m_dampingRatio * omega; // float
|
|
// Spring stiffness
|
|
var k = this.m_springMass * omega * omega; // float
|
|
// magic formulas
|
|
var h = step.dt; // float
|
|
this.m_gamma = h * (damp + h * k);
|
|
if (this.m_gamma > 0.0) {
|
|
this.m_gamma = 1.0 / this.m_gamma;
|
|
}
|
|
this.m_bias = C * h * k * this.m_gamma;
|
|
this.m_springMass = invMass + this.m_gamma;
|
|
if (this.m_springMass > 0.0) {
|
|
this.m_springMass = 1.0 / this.m_springMass;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
this.m_springImpulse = 0.0;
|
|
}
|
|
// Rotational motor
|
|
if (this.m_enableMotor) {
|
|
this.m_motorMass = iA + iB;
|
|
if (this.m_motorMass > 0.0) {
|
|
this.m_motorMass = 1.0 / this.m_motorMass;
|
|
}
|
|
}
|
|
else {
|
|
this.m_motorMass = 0.0;
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
if (step.warmStarting) {
|
|
// Account for variable time step.
|
|
this.m_impulse *= step.dtRatio;
|
|
this.m_springImpulse *= step.dtRatio;
|
|
this.m_motorImpulse *= step.dtRatio;
|
|
var P = Vec2.combine(this.m_impulse, this.m_ay, this.m_springImpulse, this.m_ax);
|
|
var LA = this.m_impulse * this.m_sAy + this.m_springImpulse * this.m_sAx + this.m_motorImpulse;
|
|
var LB = this.m_impulse * this.m_sBy + this.m_springImpulse * this.m_sBx + this.m_motorImpulse;
|
|
vA.subMul(this.m_invMassA, P);
|
|
wA -= this.m_invIA * LA;
|
|
vB.addMul(this.m_invMassB, P);
|
|
wB += this.m_invIB * LB;
|
|
}
|
|
else {
|
|
this.m_impulse = 0.0;
|
|
this.m_springImpulse = 0.0;
|
|
this.m_motorImpulse = 0.0;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
WheelJoint.prototype.solveVelocityConstraints = function (step) {
|
|
var mA = this.m_invMassA;
|
|
var mB = this.m_invMassB; // float
|
|
var iA = this.m_invIA;
|
|
var iB = this.m_invIB; // float
|
|
var vA = this.m_bodyA.c_velocity.v;
|
|
var wA = this.m_bodyA.c_velocity.w;
|
|
var vB = this.m_bodyB.c_velocity.v;
|
|
var wB = this.m_bodyB.c_velocity.w;
|
|
// Solve spring constraint
|
|
{
|
|
var Cdot = Vec2.dot(this.m_ax, vB) - Vec2.dot(this.m_ax, vA) + this.m_sBx
|
|
* wB - this.m_sAx * wA; // float
|
|
var impulse = -this.m_springMass
|
|
* (Cdot + this.m_bias + this.m_gamma * this.m_springImpulse); // float
|
|
this.m_springImpulse += impulse;
|
|
var P = Vec2.mul(impulse, this.m_ax); // Vec2
|
|
var LA = impulse * this.m_sAx; // float
|
|
var LB = impulse * this.m_sBx; // float
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
// Solve rotational motor constraint
|
|
{
|
|
var Cdot = wB - wA - this.m_motorSpeed; // float
|
|
var impulse = -this.m_motorMass * Cdot; // float
|
|
var oldImpulse = this.m_motorImpulse; // float
|
|
var maxImpulse = step.dt * this.m_maxMotorTorque; // float
|
|
this.m_motorImpulse = math.clamp(this.m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
|
impulse = this.m_motorImpulse - oldImpulse;
|
|
wA -= iA * impulse;
|
|
wB += iB * impulse;
|
|
}
|
|
// Solve point to line constraint
|
|
{
|
|
var Cdot = Vec2.dot(this.m_ay, vB) - Vec2.dot(this.m_ay, vA) + this.m_sBy
|
|
* wB - this.m_sAy * wA; // float
|
|
var impulse = -this.m_mass * Cdot; // float
|
|
this.m_impulse += impulse;
|
|
var P = Vec2.mul(impulse, this.m_ay); // Vec2
|
|
var LA = impulse * this.m_sAy; // float
|
|
var LB = impulse * this.m_sBy; // float
|
|
vA.subMul(mA, P);
|
|
wA -= iA * LA;
|
|
vB.addMul(mB, P);
|
|
wB += iB * LB;
|
|
}
|
|
this.m_bodyA.c_velocity.v.set(vA);
|
|
this.m_bodyA.c_velocity.w = wA;
|
|
this.m_bodyB.c_velocity.v.set(vB);
|
|
this.m_bodyB.c_velocity.w = wB;
|
|
};
|
|
/**
|
|
* This returns true if the position errors are within tolerance.
|
|
*/
|
|
WheelJoint.prototype.solvePositionConstraints = function (step) {
|
|
var cA = this.m_bodyA.c_position.c;
|
|
var aA = this.m_bodyA.c_position.a;
|
|
var cB = this.m_bodyB.c_position.c;
|
|
var aB = this.m_bodyB.c_position.a;
|
|
var qA = Rot.neo(aA);
|
|
var qB = Rot.neo(aB);
|
|
var rA = Rot.mulVec2(qA, Vec2.sub(this.m_localAnchorA, this.m_localCenterA));
|
|
var rB = Rot.mulVec2(qB, Vec2.sub(this.m_localAnchorB, this.m_localCenterB));
|
|
var d = Vec2.zero();
|
|
d.addCombine(1, cB, 1, rB);
|
|
d.subCombine(1, cA, 1, rA);
|
|
var ay = Rot.mulVec2(qA, this.m_localYAxisA);
|
|
var sAy = Vec2.cross(Vec2.add(d, rA), ay); // float
|
|
var sBy = Vec2.cross(rB, ay); // float
|
|
var C = Vec2.dot(d, ay); // float
|
|
var k = this.m_invMassA + this.m_invMassB + this.m_invIA * this.m_sAy
|
|
* this.m_sAy + this.m_invIB * this.m_sBy * this.m_sBy; // float
|
|
var impulse; // float
|
|
if (k != 0.0) {
|
|
impulse = -C / k;
|
|
}
|
|
else {
|
|
impulse = 0.0;
|
|
}
|
|
var P = Vec2.mul(impulse, ay); // Vec2
|
|
var LA = impulse * sAy; // float
|
|
var LB = impulse * sBy; // float
|
|
cA.subMul(this.m_invMassA, P);
|
|
aA -= this.m_invIA * LA;
|
|
cB.addMul(this.m_invMassB, P);
|
|
aB += this.m_invIB * LB;
|
|
this.m_bodyA.c_position.c.set(cA);
|
|
this.m_bodyA.c_position.a = aA;
|
|
this.m_bodyB.c_position.c.set(cB);
|
|
this.m_bodyB.c_position.a = aB;
|
|
return math.abs(C) <= Settings.linearSlop;
|
|
};
|
|
WheelJoint.TYPE = 'wheel-joint';
|
|
return WheelJoint;
|
|
}(Joint));
|
|
Joint.TYPES[WheelJoint.TYPE] = WheelJoint;
|
|
|
|
/** @deprecated Merged with main namespace */
|
|
var internal = {};
|
|
// @ts-ignore
|
|
internal.CollidePolygons = CollidePolygons;
|
|
// @ts-ignore
|
|
internal.Settings = Settings;
|
|
// @ts-ignore
|
|
internal.Sweep = Sweep;
|
|
// @ts-ignore
|
|
internal.Manifold = Manifold;
|
|
// @ts-ignore
|
|
internal.Distance = Distance;
|
|
// @ts-ignore
|
|
internal.TimeOfImpact = TimeOfImpact;
|
|
// @ts-ignore
|
|
internal.DynamicTree = DynamicTree;
|
|
// @ts-ignore
|
|
internal.stats = stats;
|
|
// @ts-ignore
|
|
Manifold.clipSegmentToLine = clipSegmentToLine;
|
|
// @ts-ignore
|
|
Manifold.ClipVertex = ClipVertex;
|
|
// @ts-ignore
|
|
Manifold.getPointStates = getPointStates;
|
|
// @ts-ignore
|
|
Manifold.PointState = PointState;
|
|
// @ts-ignore
|
|
Solver.TimeStep = TimeStep;
|
|
// @ts-ignore
|
|
Distance.testOverlap = testOverlap;
|
|
// @ts-ignore
|
|
Distance.Input = DistanceInput;
|
|
// @ts-ignore
|
|
Distance.Output = DistanceOutput;
|
|
// @ts-ignore
|
|
Distance.Proxy = DistanceProxy;
|
|
// @ts-ignore
|
|
Distance.Cache = SimplexCache;
|
|
// @ts-ignore
|
|
TimeOfImpact.Input = TOIInput;
|
|
// @ts-ignore
|
|
TimeOfImpact.Output = TOIOutput;
|
|
|
|
exports.AABB = AABB;
|
|
exports.Body = Body;
|
|
exports.Box = BoxShape;
|
|
exports.Chain = ChainShape;
|
|
exports.Circle = CircleShape;
|
|
exports.CollideCircles = CollideCircles;
|
|
exports.CollideEdgeCircle = CollideEdgeCircle;
|
|
exports.CollideEdgePolygon = CollideEdgePolygon;
|
|
exports.CollidePolygonCircle = CollidePolygonCircle;
|
|
exports.CollidePolygons = CollidePolygons;
|
|
exports.Contact = Contact;
|
|
exports.Distance = Distance;
|
|
exports.DistanceJoint = DistanceJoint;
|
|
exports.DynamicTree = DynamicTree;
|
|
exports.Edge = EdgeShape;
|
|
exports.Fixture = Fixture;
|
|
exports.FrictionJoint = FrictionJoint;
|
|
exports.GearJoint = GearJoint;
|
|
exports.Joint = Joint;
|
|
exports.Manifold = Manifold;
|
|
exports.Mat22 = Mat22;
|
|
exports.Mat33 = Mat33;
|
|
exports.Math = math;
|
|
exports.MotorJoint = MotorJoint;
|
|
exports.MouseJoint = MouseJoint;
|
|
exports.Polygon = PolygonShape;
|
|
exports.PrismaticJoint = PrismaticJoint;
|
|
exports.PulleyJoint = PulleyJoint;
|
|
exports.RevoluteJoint = RevoluteJoint;
|
|
exports.RopeJoint = RopeJoint;
|
|
exports.Rot = Rot;
|
|
exports.Serializer = Serializer;
|
|
exports.Settings = Settings;
|
|
exports.Shape = Shape;
|
|
exports.Sweep = Sweep;
|
|
exports.TimeOfImpact = TimeOfImpact;
|
|
exports.Transform = Transform;
|
|
exports.Vec2 = Vec2;
|
|
exports.Vec3 = Vec3;
|
|
exports.WeldJoint = WeldJoint;
|
|
exports.WheelJoint = WheelJoint;
|
|
exports.World = World;
|
|
exports.internal = internal;
|
|
|
|
Object.defineProperty(exports, '__esModule', { value: true });
|
|
|
|
})));
|
|
//# sourceMappingURL=planck.js.map
|