@box2d/core
Version:
A TypeScript port of Box2D
856 lines (855 loc) • 32.3 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2ValidateHull = exports.b2ComputeHull = exports.b2TestOverlap = exports.b2ClipSegmentToLine = exports.b2AABB = exports.b2RayCastOutput = exports.b2RayCastInput = exports.b2ClipVertex = exports.b2GetPointStates = exports.b2PointState = exports.b2WorldManifold = exports.b2Manifold = exports.b2ManifoldType = exports.b2ManifoldPoint = exports.b2ContactID = exports.b2ContactFeature = exports.b2ContactFeatureType = void 0;
// Copyright (c) 2019 Erin Catto
// 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.
// Structures and functions used for computing contact points, distance queries, and TOI queries.
// DEBUG: import { b2Assert } from "../common/b2_common";
const b2_common_1 = require("../common/b2_common");
const b2_math_1 = require("../common/b2_math");
const b2_distance_1 = require("./b2_distance");
const b2_settings_1 = require("../common/b2_settings");
var b2ContactFeatureType;
(function (b2ContactFeatureType) {
b2ContactFeatureType[b2ContactFeatureType["e_vertex"] = 0] = "e_vertex";
b2ContactFeatureType[b2ContactFeatureType["e_face"] = 1] = "e_face";
})(b2ContactFeatureType || (exports.b2ContactFeatureType = b2ContactFeatureType = {}));
/**
* The features that intersect to form the contact point
* This must be 4 bytes or less.
*/
class b2ContactFeature {
constructor() {
this.m_key = 0;
this.m_key_invalid = false;
/** Feature index on shapeA */
this.m_indexA = 0;
/** Feature index on shapeB */
this.m_indexB = 0;
/** The feature type on shapeA */
this.m_typeA = b2ContactFeatureType.e_vertex;
/** The feature type on shapeB */
this.m_typeB = b2ContactFeatureType.e_vertex;
}
get key() {
if (this.m_key_invalid) {
this.m_key_invalid = false;
this.m_key = this.m_indexA | (this.m_indexB << 8) | (this.m_typeA << 16) | (this.m_typeB << 24);
}
return this.m_key;
}
set key(value) {
this.m_key = value;
this.m_key_invalid = false;
this.m_indexA = this.m_key & 0xff;
this.m_indexB = (this.m_key >> 8) & 0xff;
this.m_typeA = (this.m_key >> 16) & 0xff;
this.m_typeB = (this.m_key >> 24) & 0xff;
}
get indexA() {
return this.m_indexA;
}
set indexA(value) {
this.m_indexA = value;
this.m_key_invalid = true;
}
get indexB() {
return this.m_indexB;
}
set indexB(value) {
this.m_indexB = value;
this.m_key_invalid = true;
}
get typeA() {
return this.m_typeA;
}
set typeA(value) {
this.m_typeA = value;
this.m_key_invalid = true;
}
get typeB() {
return this.m_typeB;
}
set typeB(value) {
this.m_typeB = value;
this.m_key_invalid = true;
}
}
exports.b2ContactFeature = b2ContactFeature;
/**
* Contact ids to facilitate warm starting.
*/
class b2ContactID {
constructor() {
this.cf = new b2ContactFeature();
}
Copy(o) {
this.key = o.key;
return this;
}
Clone() {
return new b2ContactID().Copy(this);
}
get key() {
return this.cf.key;
}
set key(value) {
this.cf.key = value;
}
}
exports.b2ContactID = b2ContactID;
/**
* 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.
* The local point usage depends on the 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 structure is stored across time steps, so we keep it small.
* Note: the impulses are used for internal caching and may not
* provide reliable contact forces, especially for high speed collisions.
*/
class b2ManifoldPoint {
constructor() {
/** Usage depends on manifold type */
this.localPoint = new b2_math_1.b2Vec2();
/** The non-penetration impulse */
this.normalImpulse = 0;
/** The friction impulse */
this.tangentImpulse = 0;
/** Uniquely identifies a contact point between two shapes */
this.id = new b2ContactID();
}
Reset() {
this.localPoint.SetZero();
this.normalImpulse = 0;
this.tangentImpulse = 0;
this.id.key = 0;
}
Copy(o) {
this.localPoint.Copy(o.localPoint);
this.normalImpulse = o.normalImpulse;
this.tangentImpulse = o.tangentImpulse;
this.id.Copy(o.id);
return this;
}
}
exports.b2ManifoldPoint = b2ManifoldPoint;
var b2ManifoldType;
(function (b2ManifoldType) {
b2ManifoldType[b2ManifoldType["e_circles"] = 0] = "e_circles";
b2ManifoldType[b2ManifoldType["e_faceA"] = 1] = "e_faceA";
b2ManifoldType[b2ManifoldType["e_faceB"] = 2] = "e_faceB";
})(b2ManifoldType || (exports.b2ManifoldType = b2ManifoldType = {}));
/**
* A manifold for two touching convex shapes.
* Box2D supports multiple types of contact:
* - clip point versus plane with radius
* - point versus point with radius (circles)
* The local point usage depends on the manifold type:
* -e_circles: the local center of circleA
* -e_faceA: the center of faceA
* -e_faceB: the center of faceB
* Similarly the local normal usage:
* -e_circles: not used
* -e_faceA: the normal on polygonA
* -e_faceB: the normal on polygonB
* 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.
*/
class b2Manifold {
constructor() {
/** The points of contact */
this.points = (0, b2_common_1.b2MakeArray)(b2_common_1.b2_maxManifoldPoints, b2ManifoldPoint);
/** Not use for Type::e_points */
this.localNormal = new b2_math_1.b2Vec2();
/** Usage depends on manifold type */
this.localPoint = new b2_math_1.b2Vec2();
this.type = b2ManifoldType.e_circles;
/** The number of manifold points */
this.pointCount = 0;
}
Reset() {
for (let i = 0; i < b2_common_1.b2_maxManifoldPoints; ++i) {
// DEBUG: b2Assert(this.points[i] instanceof b2ManifoldPoint);
this.points[i].Reset();
}
this.localNormal.SetZero();
this.localPoint.SetZero();
this.type = b2ManifoldType.e_circles;
this.pointCount = 0;
}
Copy(o) {
this.pointCount = o.pointCount;
for (let i = 0; i < b2_common_1.b2_maxManifoldPoints; ++i) {
// DEBUG: b2Assert(this.points[i] instanceof b2ManifoldPoint);
this.points[i].Copy(o.points[i]);
}
this.localNormal.Copy(o.localNormal);
this.localPoint.Copy(o.localPoint);
this.type = o.type;
return this;
}
Clone() {
return new b2Manifold().Copy(this);
}
}
exports.b2Manifold = b2Manifold;
/**
* This is used to compute the current state of a contact manifold.
*/
class b2WorldManifold {
constructor() {
/** World vector pointing from A to B */
this.normal = new b2_math_1.b2Vec2();
/** World contact point (point of intersection) */
this.points = (0, b2_common_1.b2MakeArray)(b2_common_1.b2_maxManifoldPoints, b2_math_1.b2Vec2);
/** A negative value indicates overlap, in meters */
this.separations = (0, b2_common_1.b2MakeNumberArray)(b2_common_1.b2_maxManifoldPoints);
}
/**
* 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.
*/
Initialize(manifold, xfA, radiusA, xfB, radiusB) {
if (manifold.pointCount === 0) {
return;
}
switch (manifold.type) {
case b2ManifoldType.e_circles: {
this.normal.Set(1, 0);
const pointA = b2_math_1.b2Transform.MultiplyVec2(xfA, manifold.localPoint, b2WorldManifold.Initialize_s_pointA);
const pointB = b2_math_1.b2Transform.MultiplyVec2(xfB, manifold.points[0].localPoint, b2WorldManifold.Initialize_s_pointB);
if (b2_math_1.b2Vec2.DistanceSquared(pointA, pointB) > b2_common_1.b2_epsilon_sq) {
b2_math_1.b2Vec2.Subtract(pointB, pointA, this.normal).Normalize();
}
const cA = b2_math_1.b2Vec2.AddScaled(pointA, radiusA, this.normal, b2WorldManifold.Initialize_s_cA);
const cB = b2_math_1.b2Vec2.SubtractScaled(pointB, radiusB, this.normal, b2WorldManifold.Initialize_s_cB);
b2_math_1.b2Vec2.Mid(cA, cB, this.points[0]);
this.separations[0] = b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(cB, cA, b2_math_1.b2Vec2.s_t0), this.normal);
break;
}
case b2ManifoldType.e_faceA: {
b2_math_1.b2Rot.MultiplyVec2(xfA.q, manifold.localNormal, this.normal);
const planePoint = b2_math_1.b2Transform.MultiplyVec2(xfA, manifold.localPoint, b2WorldManifold.Initialize_s_planePoint);
for (let i = 0; i < manifold.pointCount; ++i) {
const clipPoint = b2_math_1.b2Transform.MultiplyVec2(xfB, manifold.points[i].localPoint, b2WorldManifold.Initialize_s_clipPoint);
const s = radiusA - b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(clipPoint, planePoint, b2_math_1.b2Vec2.s_t0), this.normal);
const cA = b2_math_1.b2Vec2.AddScaled(clipPoint, s, this.normal, b2WorldManifold.Initialize_s_cA);
const cB = b2_math_1.b2Vec2.SubtractScaled(clipPoint, radiusB, this.normal, b2WorldManifold.Initialize_s_cB);
b2_math_1.b2Vec2.Mid(cA, cB, this.points[i]);
this.separations[i] = b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(cB, cA, b2_math_1.b2Vec2.s_t0), this.normal);
}
break;
}
case b2ManifoldType.e_faceB: {
b2_math_1.b2Rot.MultiplyVec2(xfB.q, manifold.localNormal, this.normal);
const planePoint = b2_math_1.b2Transform.MultiplyVec2(xfB, manifold.localPoint, b2WorldManifold.Initialize_s_planePoint);
for (let i = 0; i < manifold.pointCount; ++i) {
const clipPoint = b2_math_1.b2Transform.MultiplyVec2(xfA, manifold.points[i].localPoint, b2WorldManifold.Initialize_s_clipPoint);
const s = radiusB - b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(clipPoint, planePoint, b2_math_1.b2Vec2.s_t0), this.normal);
const cB = b2_math_1.b2Vec2.AddScaled(clipPoint, s, this.normal, b2WorldManifold.Initialize_s_cB);
const cA = b2_math_1.b2Vec2.SubtractScaled(clipPoint, radiusA, this.normal, b2WorldManifold.Initialize_s_cA);
b2_math_1.b2Vec2.Mid(cA, cB, this.points[i]);
this.separations[i] = b2_math_1.b2Vec2.Dot(b2_math_1.b2Vec2.Subtract(cA, cB, b2_math_1.b2Vec2.s_t0), this.normal);
}
// Ensure normal points from A to B.
this.normal.Negate();
break;
}
}
}
}
exports.b2WorldManifold = b2WorldManifold;
b2WorldManifold.Initialize_s_pointA = new b2_math_1.b2Vec2();
b2WorldManifold.Initialize_s_pointB = new b2_math_1.b2Vec2();
b2WorldManifold.Initialize_s_cA = new b2_math_1.b2Vec2();
b2WorldManifold.Initialize_s_cB = new b2_math_1.b2Vec2();
b2WorldManifold.Initialize_s_planePoint = new b2_math_1.b2Vec2();
b2WorldManifold.Initialize_s_clipPoint = new b2_math_1.b2Vec2();
/**
* This is used for determining the state of contact points.
*/
var b2PointState;
(function (b2PointState) {
/** Point does not exist */
b2PointState[b2PointState["b2_nullState"] = 0] = "b2_nullState";
/** Point was added in the update */
b2PointState[b2PointState["b2_addState"] = 1] = "b2_addState";
/** Point persisted across the update */
b2PointState[b2PointState["b2_persistState"] = 2] = "b2_persistState";
/** Point was removed in the update */
b2PointState[b2PointState["b2_removeState"] = 3] = "b2_removeState";
})(b2PointState || (exports.b2PointState = b2PointState = {}));
/**
* 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 b2GetPointStates(state1, state2, manifold1, manifold2) {
// Detect persists and removes.
let i;
for (i = 0; i < manifold1.pointCount; ++i) {
const { key } = manifold1.points[i].id;
state1[i] = b2PointState.b2_removeState;
for (let j = 0; j < manifold2.pointCount; ++j) {
if (manifold2.points[j].id.key === key) {
state1[i] = b2PointState.b2_persistState;
break;
}
}
}
for (; i < b2_common_1.b2_maxManifoldPoints; ++i) {
state1[i] = b2PointState.b2_nullState;
}
// Detect persists and adds.
for (i = 0; i < manifold2.pointCount; ++i) {
const { key } = manifold2.points[i].id;
state2[i] = b2PointState.b2_addState;
for (let j = 0; j < manifold1.pointCount; ++j) {
if (manifold1.points[j].id.key === key) {
state2[i] = b2PointState.b2_persistState;
break;
}
}
}
for (; i < b2_common_1.b2_maxManifoldPoints; ++i) {
state2[i] = b2PointState.b2_nullState;
}
}
exports.b2GetPointStates = b2GetPointStates;
/**
* Used for computing contact manifolds.
*/
class b2ClipVertex {
constructor() {
this.v = new b2_math_1.b2Vec2();
this.id = new b2ContactID();
}
Copy(other) {
this.v.Copy(other.v);
this.id.Copy(other.id);
return this;
}
}
exports.b2ClipVertex = b2ClipVertex;
/**
* Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
*/
class b2RayCastInput {
constructor() {
this.p1 = new b2_math_1.b2Vec2();
this.p2 = new b2_math_1.b2Vec2();
this.maxFraction = 1;
}
Copy(o) {
this.p1.Copy(o.p1);
this.p2.Copy(o.p2);
this.maxFraction = o.maxFraction;
return this;
}
}
exports.b2RayCastInput = b2RayCastInput;
/**
* Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
* come from b2RayCastInput.
*/
class b2RayCastOutput {
constructor() {
this.normal = new b2_math_1.b2Vec2();
this.fraction = 0;
}
Copy(o) {
this.normal.Copy(o.normal);
this.fraction = o.fraction;
return this;
}
}
exports.b2RayCastOutput = b2RayCastOutput;
/**
* An axis aligned bounding box.
*/
class b2AABB {
constructor() {
/** The lower vertex */
this.lowerBound = new b2_math_1.b2Vec2();
/** The upper vertex */
this.upperBound = new b2_math_1.b2Vec2();
}
Copy(o) {
this.lowerBound.Copy(o.lowerBound);
this.upperBound.Copy(o.upperBound);
return this;
}
/**
* Verify that the bounds are sorted.
*/
IsValid() {
return (this.lowerBound.IsValid() &&
this.upperBound.IsValid() &&
this.upperBound.x >= this.lowerBound.x &&
this.upperBound.y >= this.lowerBound.y);
}
/**
* Get the center of the AABB.
*/
GetCenter(out) {
return b2_math_1.b2Vec2.Mid(this.lowerBound, this.upperBound, out);
}
/**
* Get the extents of the AABB (half-widths).
*/
GetExtents(out) {
return b2_math_1.b2Vec2.Extents(this.lowerBound, this.upperBound, out);
}
/**
* Get the perimeter length
*/
GetPerimeter() {
const wx = this.upperBound.x - this.lowerBound.x;
const wy = this.upperBound.y - this.lowerBound.y;
return 2 * (wx + wy);
}
/**
* Combine an AABB into this one.
*/
Combine1(aabb) {
this.lowerBound.x = Math.min(this.lowerBound.x, aabb.lowerBound.x);
this.lowerBound.y = Math.min(this.lowerBound.y, aabb.lowerBound.y);
this.upperBound.x = Math.max(this.upperBound.x, aabb.upperBound.x);
this.upperBound.y = Math.max(this.upperBound.y, aabb.upperBound.y);
return this;
}
/**
* Combine two AABBs into this one.
*/
Combine2(aabb1, aabb2) {
this.lowerBound.x = Math.min(aabb1.lowerBound.x, aabb2.lowerBound.x);
this.lowerBound.y = Math.min(aabb1.lowerBound.y, aabb2.lowerBound.y);
this.upperBound.x = Math.max(aabb1.upperBound.x, aabb2.upperBound.x);
this.upperBound.y = Math.max(aabb1.upperBound.y, aabb2.upperBound.y);
return this;
}
static Combine(aabb1, aabb2, out) {
out.Combine2(aabb1, aabb2);
return out;
}
/**
* Does this aabb contain the provided AABB.
*/
Contains(aabb) {
return (this.lowerBound.x <= aabb.lowerBound.x &&
this.lowerBound.y <= aabb.lowerBound.y &&
aabb.upperBound.x <= this.upperBound.x &&
aabb.upperBound.y <= this.upperBound.y);
}
// From Real-time Collision Detection, p179.
RayCast(output, input) {
let tmin = -b2_common_1.b2_maxFloat;
let tmax = b2_common_1.b2_maxFloat;
const p_x = input.p1.x;
const p_y = input.p1.y;
const d_x = input.p2.x - input.p1.x;
const d_y = input.p2.y - input.p1.y;
const absD_x = Math.abs(d_x);
const absD_y = Math.abs(d_y);
const { normal } = output;
if (absD_x < b2_common_1.b2_epsilon) {
// Parallel.
if (p_x < this.lowerBound.x || this.upperBound.x < p_x) {
return false;
}
}
else {
const inv_d = 1 / d_x;
let t1 = (this.lowerBound.x - p_x) * inv_d;
let t2 = (this.upperBound.x - p_x) * inv_d;
// Sign of the normal vector.
let s = -1;
if (t1 > t2) {
const t3 = t1;
t1 = t2;
t2 = t3;
s = 1;
}
// Push the min up
if (t1 > tmin) {
normal.x = s;
normal.y = 0;
tmin = t1;
}
// Pull the max down
tmax = Math.min(tmax, t2);
if (tmin > tmax) {
return false;
}
}
if (absD_y < b2_common_1.b2_epsilon) {
// Parallel.
if (p_y < this.lowerBound.y || this.upperBound.y < p_y) {
return false;
}
}
else {
const inv_d = 1 / d_y;
let t1 = (this.lowerBound.y - p_y) * inv_d;
let t2 = (this.upperBound.y - p_y) * inv_d;
// Sign of the normal vector.
let s = -1;
if (t1 > t2) {
const t3 = t1;
t1 = t2;
t2 = t3;
s = 1;
}
// Push the min up
if (t1 > tmin) {
normal.x = 0;
normal.y = 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 || input.maxFraction < tmin) {
return false;
}
// Intersection.
output.fraction = tmin;
return true;
}
TestContain(point) {
if (point.x < this.lowerBound.x || this.upperBound.x < point.x) {
return false;
}
if (point.y < this.lowerBound.y || this.upperBound.y < point.y) {
return false;
}
return true;
}
TestOverlap(other) {
if (this.upperBound.x < other.lowerBound.x) {
return false;
}
if (this.upperBound.y < other.lowerBound.y) {
return false;
}
if (other.upperBound.x < this.lowerBound.x) {
return false;
}
if (other.upperBound.y < this.lowerBound.y) {
return false;
}
return true;
}
}
exports.b2AABB = b2AABB;
/**
* Clipping for contact manifolds.
* Sutherland-Hodgman clipping.
*/
function b2ClipSegmentToLine(vOut, [vIn0, vIn1], normal, offset, vertexIndexA) {
// Start with no output points
let count = 0;
// Calculate the distance of end points to the line
const distance0 = b2_math_1.b2Vec2.Dot(normal, vIn0.v) - offset;
const distance1 = b2_math_1.b2Vec2.Dot(normal, vIn1.v) - offset;
// If the points are behind the plane
if (distance0 <= 0)
vOut[count++].Copy(vIn0);
if (distance1 <= 0)
vOut[count++].Copy(vIn1);
// If the points are on different sides of the plane
if (distance0 * distance1 < 0) {
// Find intersection point of edge and plane
const interp = distance0 / (distance0 - distance1);
const { v, id } = vOut[count];
v.x = vIn0.v.x + interp * (vIn1.v.x - vIn0.v.x);
v.y = vIn0.v.y + interp * (vIn1.v.y - vIn0.v.y);
// VertexA is hitting edgeB.
id.cf.indexA = vertexIndexA;
id.cf.indexB = vIn0.id.cf.indexB;
id.cf.typeA = b2ContactFeatureType.e_vertex;
id.cf.typeB = b2ContactFeatureType.e_face;
++count;
// b2Assert(count === 2);
}
return count;
}
exports.b2ClipSegmentToLine = b2ClipSegmentToLine;
const b2TestOverlap_s_input = new b2_distance_1.b2DistanceInput();
const b2TestOverlap_s_simplexCache = new b2_distance_1.b2SimplexCache();
const b2TestOverlap_s_output = new b2_distance_1.b2DistanceOutput();
/**
* Determine if two generic shapes overlap.
*/
function b2TestOverlap(shapeA, indexA, shapeB, indexB, xfA, xfB) {
const input = b2TestOverlap_s_input.Reset();
input.proxyA.SetShape(shapeA, indexA);
input.proxyB.SetShape(shapeB, indexB);
input.transformA.Copy(xfA);
input.transformB.Copy(xfB);
input.useRadii = true;
const simplexCache = b2TestOverlap_s_simplexCache.Reset();
simplexCache.count = 0;
const output = b2TestOverlap_s_output.Reset();
(0, b2_distance_1.b2Distance)(output, simplexCache, input);
return output.distance < 10 * b2_common_1.b2_epsilon;
}
exports.b2TestOverlap = b2TestOverlap;
const b2RecurseHull_s_e = new b2_math_1.b2Vec2();
const b2RecurseHull_s_c = new b2_math_1.b2Vec2();
const emptyHull = [];
/** quickhull recursion */
function b2RecurseHull(p1, p2, ps) {
if (ps.length === 0) {
return emptyHull;
}
// create an edge vector pointing from p1 to p2
const e = b2_math_1.b2Vec2.Subtract(p2, p1, b2RecurseHull_s_e);
e.Normalize();
// discard points left of e and find point furthest to the right of e
const rightPoints = [];
let bestIndex = 0;
let bestDistance = b2_math_1.b2Vec2.Cross(b2_math_1.b2Vec2.Subtract(ps[bestIndex], p1, b2RecurseHull_s_c), e);
if (bestDistance > 0) {
rightPoints.push(ps[bestIndex]);
}
for (let i = 1; i < ps.length; ++i) {
const distance = b2_math_1.b2Vec2.Cross(b2_math_1.b2Vec2.Subtract(ps[i], p1, b2RecurseHull_s_c), e);
if (distance > bestDistance) {
bestIndex = i;
bestDistance = distance;
}
if (distance > 0) {
rightPoints.push(ps[i]);
}
}
if (bestDistance < 2 * b2_common_1.b2_linearSlop) {
return emptyHull;
}
const bestPoint = ps[bestIndex];
// compute hull to the right of p1-bestPoint
const hull1 = b2RecurseHull(p1, bestPoint, rightPoints);
// compute hull to the right of bestPoint-p2
const hull2 = b2RecurseHull(bestPoint, p2, rightPoints);
// stich together hulls
const hull = [...hull1, bestPoint, ...hull2];
(0, b2_common_1.b2Assert)(hull.length < b2_settings_1.b2_maxPolygonVertices);
return hull;
}
const b2ComputeHull_s_e = new b2_math_1.b2Vec2();
const b2ComputeHull_s_v = new b2_math_1.b2Vec2();
const b2ComputeHull_s_c = new b2_math_1.b2Vec2();
const b2ComputeHull_s_d = new b2_math_1.b2Vec2();
const b2ComputeHull_s_aabb = new b2AABB();
/**
* Compute the convex hull of a set of points.
* quickhull algorithm
* - merges vertices based on b2_linearSlop
* - removes collinear points using b2_linearSlop
* - returns an empty hull if it fails
*
* Some failure cases:
* - all points very close together
* - all points on a line
* - less than 3 points
* - more than b2_maxPolygonVertices points
*
* This welds close points and removes collinear points.
*
* @returns an empty hull if it fails.
*/
function b2ComputeHull(points, count) {
if (count < 3 || count > b2_settings_1.b2_maxPolygonVertices) {
// check your data
return emptyHull;
}
count = Math.min(count, b2_settings_1.b2_maxPolygonVertices);
const aabb = b2ComputeHull_s_aabb;
aabb.lowerBound.Set(b2_common_1.b2_maxFloat, b2_common_1.b2_maxFloat);
aabb.upperBound.Set(-b2_common_1.b2_maxFloat, -b2_common_1.b2_maxFloat);
// Perform aggressive point welding. First point always remains.
// Also compute the bounding box for later.
const ps = [];
const tolSqr = 16 * b2_common_1.b2_linearSlop * b2_common_1.b2_linearSlop;
for (let i = 0; i < count; ++i) {
b2_math_1.b2Vec2.Min(aabb.lowerBound, points[i], aabb.lowerBound);
b2_math_1.b2Vec2.Max(aabb.upperBound, points[i], aabb.upperBound);
const vi = points[i];
let unique = true;
for (let j = 0; j < i; ++j) {
const vj = points[j];
const distSqr = b2_math_1.b2Vec2.DistanceSquared(vi, vj);
if (distSqr < tolSqr) {
unique = false;
break;
}
}
if (unique) {
ps.push(vi);
}
}
let n = ps.length;
if (n < 3) {
// all points very close together, check your data and check your scale
return emptyHull;
}
// Find an extreme point as the first point on the hull
const c = aabb.GetCenter(b2ComputeHull_s_c);
let i1 = 0;
let dsq1 = b2_math_1.b2Vec2.DistanceSquared(c, ps[i1]);
for (let i = 1; i < n; ++i) {
const dsq = b2_math_1.b2Vec2.DistanceSquared(c, ps[i]);
if (dsq > dsq1) {
i1 = i;
dsq1 = dsq;
}
}
// remove p1 from working set
const p1 = ps[i1];
ps[i1] = ps[n - 1];
n -= 1;
let i2 = 0;
let dsq2 = b2_math_1.b2Vec2.DistanceSquared(p1, ps[i2]);
for (let i = 1; i < n; ++i) {
const dsq = b2_math_1.b2Vec2.DistanceSquared(p1, ps[i]);
if (dsq > dsq2) {
i2 = i;
dsq2 = dsq;
}
}
// remove p2 from working set
const p2 = ps[i2];
ps[i2] = ps[n - 1];
n -= 1;
// split the points into points that are left and right of the line p1-p2.
const rightPoints = [];
const leftPoints = [];
const e = b2_math_1.b2Vec2.Subtract(p2, p1, b2ComputeHull_s_e);
e.Normalize();
for (let i = 0; i < n; ++i) {
const d = b2_math_1.b2Vec2.Cross(b2_math_1.b2Vec2.Subtract(ps[i], p1, b2ComputeHull_s_d), e);
// slop used here to skip points that are very close to the line p1-p2
if (d >= 2 * b2_common_1.b2_linearSlop) {
rightPoints.push(ps[i]);
}
else if (d <= -2 * b2_common_1.b2_linearSlop) {
leftPoints.push(ps[i]);
}
}
// compute hulls on right and left
const hull1 = b2RecurseHull(p1, p2, rightPoints);
const hull2 = b2RecurseHull(p2, p1, leftPoints);
if (hull1.length === 0 && hull2.length === 0) {
// all points collinear
return emptyHull;
}
// stitch hulls together, preserving CCW winding order
const hull = [p1, ...hull1, p2, ...hull2];
(0, b2_common_1.b2Assert)(hull.length <= b2_settings_1.b2_maxPolygonVertices);
// merge collinear
let searching = true;
while (searching && hull.length > 2) {
searching = false;
for (let i = 0; i < hull.length; ++i) {
const i1b = i;
const i2b = (i + 1) % hull.length;
const i3b = (i + 2) % hull.length;
const p1b = hull[i1b];
const p2b = hull[i2b];
const p3b = hull[i3b];
const eb = b2_math_1.b2Vec2.Subtract(p3b, p1b, b2ComputeHull_s_e);
eb.Normalize();
const v = b2_math_1.b2Vec2.Subtract(p2b, p1b, b2ComputeHull_s_v);
const distance = b2_math_1.b2Vec2.Cross(v, eb);
if (distance <= 2 * b2_common_1.b2_linearSlop) {
// remove midpoint from hull
hull.splice(i2b, 1);
// continue searching for collinear points
searching = true;
break;
}
}
}
if (hull.length < 3) {
// all points collinear, shouldn't be reached since this was validated above
hull.length = 0;
}
return hull;
}
exports.b2ComputeHull = b2ComputeHull;
const b2ValidateHull_s_e = new b2_math_1.b2Vec2();
const b2ValidateHull_s_d = new b2_math_1.b2Vec2();
/**
* This determines if a hull is valid. Checks for:
* - convexity
* - collinear points
* This is expensive and should not be called at runtime.
*/
function b2ValidateHull(hull, count) {
if (count < 3 || b2_settings_1.b2_maxPolygonVertices < count) {
return false;
}
// test that every point is behind every edge
for (let i = 0; i < count; ++i) {
// create an edge vector
const i1 = i;
const i2 = i < count - 1 ? i1 + 1 : 0;
const p = hull[i1];
const e = b2_math_1.b2Vec2.Subtract(hull[i2], p, b2ValidateHull_s_e);
e.Normalize();
for (let j = 0; j < count; ++j) {
// skip points that subtend the current edge
if (j === i1 || j === i2) {
continue;
}
const distance = b2_math_1.b2Vec2.Cross(b2_math_1.b2Vec2.Subtract(hull[j], p, b2ValidateHull_s_d), e);
if (distance >= 0) {
return false;
}
}
}
// test for collinear points
for (let i = 0; i < count; ++i) {
const i1 = i;
const i2 = (i + 1) % count;
const i3 = (i + 2) % count;
const p1 = hull[i1];
const p2 = hull[i2];
const p3 = hull[i3];
const e = b2_math_1.b2Vec2.Subtract(p3, p1, b2ValidateHull_s_e);
e.Normalize();
const distance = b2_math_1.b2Vec2.Cross(b2_math_1.b2Vec2.Subtract(p2, p1, b2ValidateHull_s_d), e);
if (distance <= b2_common_1.b2_linearSlop) {
// p1-p2-p3 are collinear
return false;
}
}
return true;
}
exports.b2ValidateHull = b2ValidateHull;