@perplexdotgg/bounce 1.0.0 → 1.0.1
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/build/bounce.d.ts +39501 -0
- package/build/bounce.js +17166 -0
- package/package.json +1 -1
- package/src/builders/ConvexHullBuilder.ts +0 -437
- package/src/builders/ConvexHullBuilder2d.ts +0 -344
- package/src/builders/ConvexHullBuilder3d.ts +0 -1689
- package/src/builders/HeightMapBuilder.ts +0 -414
- package/src/builders/TriangleMeshBuilder.ts +0 -92
- package/src/collision/CastShapesModule.ts +0 -184
- package/src/collision/CollideShapesModule.ts +0 -152
- package/src/collision/HeightMapCaster.ts +0 -38
- package/src/collision/HeightMapCollider.ts +0 -33
- package/src/collision/TriangleCaster.ts +0 -249
- package/src/collision/TriangleCollider.ts +0 -308
- package/src/collision/TriangleCollider2.ts +0 -379
- package/src/collision/activeEdge.ts +0 -146
- package/src/collision/cast/cast.ts +0 -139
- package/src/collision/cast/castCompoundVsCompound.ts +0 -59
- package/src/collision/cast/castCompoundVsConvex.ts +0 -116
- package/src/collision/cast/castConvexVsCompound.ts +0 -123
- package/src/collision/cast/castConvexVsConvex.ts +0 -213
- package/src/collision/cast/castConvexVsHeightMap.ts +0 -73
- package/src/collision/cast/castConvexVsTriangleMesh.ts +0 -56
- package/src/collision/cast/castRayVsCompound.ts +0 -44
- package/src/collision/cast/castRayVsConvex.ts +0 -45
- package/src/collision/cast/castRayVsHeightMap.ts +0 -58
- package/src/collision/cast/castRayVsTriangleMesh.ts +0 -58
- package/src/collision/closestPoints/closestPoints.ts +0 -23
- package/src/collision/closestPoints/computeBarycentricCoordinates2d.ts +0 -32
- package/src/collision/closestPoints/computeBarycentricCoordinates3d.ts +0 -81
- package/src/collision/closestPoints/computeClosestPointOnLine.ts +0 -30
- package/src/collision/closestPoints/computeClosestPointOnTetrahedron.ts +0 -96
- package/src/collision/closestPoints/computeClosestPointOnTriangle.ts +0 -195
- package/src/collision/closestPoints/isOriginOutsideOfPlane.ts +0 -25
- package/src/collision/closestPoints/isOriginOutsideOfTrianglePlanes.ts +0 -72
- package/src/collision/collide/collide.ts +0 -146
- package/src/collision/collide/collideCompoundVsCompound.ts +0 -60
- package/src/collision/collide/collideCompoundVsConvex.ts +0 -59
- package/src/collision/collide/collideCompoundVsHeightMap.ts +0 -73
- package/src/collision/collide/collideCompoundVsTriangleMesh.ts +0 -56
- package/src/collision/collide/collideConvexVsCompound.ts +0 -57
- package/src/collision/collide/collideConvexVsConvex.ts +0 -225
- package/src/collision/collide/collideConvexVsConvexImp.ts +0 -236
- package/src/collision/collide/collideConvexVsHeightMap.ts +0 -53
- package/src/collision/collide/collideConvexVsTriangleMesh.ts +0 -58
- package/src/collision/collide/collideHeightMapVsCompound.ts +0 -69
- package/src/collision/collide/collideHeightMapVsConvex.ts +0 -53
- package/src/collision/collide/collideSphereVsSphere.ts +0 -81
- package/src/collision/collide/collideTriangleMeshVsCompound.ts +0 -58
- package/src/collision/collide/collideTriangleMeshVsConvex.ts +0 -58
- package/src/collision/epa/EpaConvexHullBuilder.ts +0 -397
- package/src/collision/epa/StaticArray.ts +0 -154
- package/src/collision/epa/TriangleFactory.ts +0 -32
- package/src/collision/epa/arrays.ts +0 -99
- package/src/collision/epa/binaryHeap.ts +0 -82
- package/src/collision/epa/structs.ts +0 -227
- package/src/collision/gjk/GjkModule.ts +0 -864
- package/src/collision/gjk/PenetrationDepthModule.ts +0 -493
- package/src/collision/gjk/SupportPoints.ts +0 -50
- package/src/collision/imp/MinkowskiDifference.ts +0 -36
- package/src/collision/imp/computeExploredDistanceLowerUpperBound.ts +0 -40
- package/src/collision/imp/finalizeImpResult.ts +0 -69
- package/src/collision/imp/findContactImp.ts +0 -196
- package/src/collision/imp/imp.ts +0 -28
- package/src/collision/imp/incrementalMinimumDistanceExploreDirection.ts +0 -207
- package/src/collision/mpr/findPortal.ts +0 -152
- package/src/collision/mpr/mpr.ts +0 -29
- package/src/collision/mpr/updatePortal.ts +0 -52
- package/src/constraints/BaseConstraint.ts +0 -50
- package/src/constraints/ConstraintOptions.ts +0 -22
- package/src/constraints/ConstraintSolver.ts +0 -119
- package/src/constraints/DistanceConstraint.ts +0 -229
- package/src/constraints/FixedConstraint.ts +0 -203
- package/src/constraints/HingeConstraint.ts +0 -460
- package/src/constraints/PointConstraint.ts +0 -108
- package/src/constraints/components/AngleComponent.ts +0 -226
- package/src/constraints/components/AxisComponent.ts +0 -263
- package/src/constraints/components/HingeComponent.ts +0 -215
- package/src/constraints/components/Motor.ts +0 -36
- package/src/constraints/components/PointConstraintComponent.ts +0 -179
- package/src/constraints/components/RotationEulerComponent.ts +0 -139
- package/src/constraints/components/Spring.ts +0 -30
- package/src/constraints/components/SpringComponent.ts +0 -71
- package/src/constraints/types.ts +0 -6
- package/src/helpers.ts +0 -147
- package/src/index.ts +0 -50
- package/src/math/BasicTransform.ts +0 -19
- package/src/math/NumberValue.ts +0 -13
- package/src/math/isometry.ts +0 -64
- package/src/math/mat3.ts +0 -529
- package/src/math/mat4.ts +0 -588
- package/src/math/quat.ts +0 -193
- package/src/math/scalar.ts +0 -81
- package/src/math/tensor.ts +0 -17
- package/src/math/vec3.ts +0 -589
- package/src/math/vec4.ts +0 -10
- package/src/physics/Body.ts +0 -581
- package/src/physics/CollisionFilter.ts +0 -52
- package/src/physics/SleepModule.ts +0 -163
- package/src/physics/broadphase/BodyPairsModule.ts +0 -363
- package/src/physics/broadphase/BvhModule.ts +0 -237
- package/src/physics/broadphase/BvhTree.ts +0 -803
- package/src/physics/broadphase/ConstraintPairsModule.ts +0 -385
- package/src/physics/broadphase/TriangleMeshBvhTree.ts +0 -379
- package/src/physics/manifold/ContactManifold.ts +0 -227
- package/src/physics/manifold/ContactManifoldModule.ts +0 -623
- package/src/physics/manifold/Face.ts +0 -119
- package/src/physics/manifold/ManifoldCache.ts +0 -116
- package/src/physics/manifold/clipping/clipPolyVsEdge.ts +0 -131
- package/src/physics/manifold/clipping/clipPolyVsPlane.ts +0 -73
- package/src/physics/manifold/clipping/clipPolyVsPoly.ts +0 -72
- package/src/physics/narrowphase/CollideBodiesModule.ts +0 -755
- package/src/physics/solver/ContactConstraintModule.ts +0 -659
- package/src/physics/solver/ManifoldConstraint.ts +0 -420
- package/src/physics/solver/estimateCollisionResponse.ts +0 -146
- package/src/shape/Aabb.ts +0 -400
- package/src/shape/Box.ts +0 -231
- package/src/shape/Capsule.ts +0 -332
- package/src/shape/CompoundShape.ts +0 -288
- package/src/shape/Convex.ts +0 -130
- package/src/shape/ConvexHull.ts +0 -423
- package/src/shape/Cylinder.ts +0 -313
- package/src/shape/HeightMap.ts +0 -511
- package/src/shape/Line.ts +0 -14
- package/src/shape/Plane.ts +0 -116
- package/src/shape/Ray.ts +0 -81
- package/src/shape/Segment.ts +0 -25
- package/src/shape/Shape.ts +0 -77
- package/src/shape/Sphere.ts +0 -181
- package/src/shape/TransformedShape.ts +0 -51
- package/src/shape/Triangle.ts +0 -122
- package/src/shape/TriangleMesh.ts +0 -186
- package/src/types.ts +0 -1
- package/src/world.ts +0 -1335
|
@@ -1,623 +0,0 @@
|
|
|
1
|
-
import { createClass, NumberType, ReferenceListType, PropertyDefinitionMap, NumberArray, PoolClass } from "monomorph";
|
|
2
|
-
import { Quat } from "../../math/quat";
|
|
3
|
-
import { degreesToRadians, squared } from "../../math/scalar";
|
|
4
|
-
import { Vec3 } from "../../math/vec3";
|
|
5
|
-
import { Body, BodyType } from "../Body";
|
|
6
|
-
import { clipPolyVsEdge } from "./clipping/clipPolyVsEdge";
|
|
7
|
-
import { clipPolyVsPoly } from "./clipping/clipPolyVsPoly";
|
|
8
|
-
import { ContactManifold, ContactManifoldPool, createContactPairKey } from "./ContactManifold";
|
|
9
|
-
import { ContactPair } from "./ContactManifold";
|
|
10
|
-
import { Face } from "./Face";
|
|
11
|
-
import { ManifoldCache, ManifoldCacheOptions } from "./ManifoldCache";
|
|
12
|
-
import { Mat4 } from "../../math/mat4";
|
|
13
|
-
import { ContactConstraintModule } from "../solver/ContactConstraintModule";
|
|
14
|
-
import { SleepModule } from "../SleepModule";
|
|
15
|
-
|
|
16
|
-
const contactPairCacheMaxDeltaPositionSquared = squared(0.001); // < 1mm
|
|
17
|
-
const contactPairCacheCosMaxDeltaRotationDiv2 = Math.cos(degreesToRadians(2) / 2);
|
|
18
|
-
const negatedPenetrationAxis = /*@__PURE__*/ Vec3.create();
|
|
19
|
-
|
|
20
|
-
const vec3BufferProps = {
|
|
21
|
-
numItems: NumberType(0),
|
|
22
|
-
vec3List: ReferenceListType(Vec3),
|
|
23
|
-
} as const satisfies PropertyDefinitionMap;
|
|
24
|
-
|
|
25
|
-
export class Vec3Buffer extends createClass<Vec3Buffer, typeof vec3BufferProps>(vec3BufferProps) {
|
|
26
|
-
getVec3(out: Vec3, index: number): void {
|
|
27
|
-
out.copy(this.vec3List!.getAtIndex(index)!);
|
|
28
|
-
}
|
|
29
|
-
|
|
30
|
-
pushVec3(value: Vec3): void {
|
|
31
|
-
if (this.numItems >= this.vec3List!.maxLength) {
|
|
32
|
-
throw new Error("Vec3Buffer is full");
|
|
33
|
-
}
|
|
34
|
-
|
|
35
|
-
this.vec3List!.getAtIndex(this.numItems)!.copy(value);
|
|
36
|
-
this.numItems++;
|
|
37
|
-
}
|
|
38
|
-
|
|
39
|
-
getNumber(index: number) {
|
|
40
|
-
const vec = this.vec3List!.getAtIndex(index)!;
|
|
41
|
-
return vec.x;
|
|
42
|
-
}
|
|
43
|
-
|
|
44
|
-
pushNumber(value: number) {
|
|
45
|
-
if (this.numItems >= this.vec3List!.maxLength) {
|
|
46
|
-
throw new Error("Vec3Buffer is full");
|
|
47
|
-
}
|
|
48
|
-
|
|
49
|
-
const vec = this.vec3List!.getAtIndex(this.numItems)!;
|
|
50
|
-
vec.x = value;
|
|
51
|
-
this.numItems++;
|
|
52
|
-
}
|
|
53
|
-
|
|
54
|
-
clear(): void {
|
|
55
|
-
this.numItems = 0;
|
|
56
|
-
}
|
|
57
|
-
}
|
|
58
|
-
|
|
59
|
-
const oldCreate = Vec3Buffer.create;
|
|
60
|
-
Vec3Buffer.create = function () {
|
|
61
|
-
const vec3Buffer = oldCreate.apply(this, arguments as any);
|
|
62
|
-
|
|
63
|
-
// todo this behavior probably should change?
|
|
64
|
-
// currently, we create all the contactConstraints once the first time, and never destroy them.
|
|
65
|
-
// they technically should be destroyed, but this is more efficient for this use case
|
|
66
|
-
// since we will be reusing the same manifolds over and over again, so we should just
|
|
67
|
-
// increment a numManifolds instead that is reset to 0
|
|
68
|
-
if (vec3Buffer.vec3List.length >= vec3Buffer.vec3List!.maxLength) {
|
|
69
|
-
return vec3Buffer;
|
|
70
|
-
}
|
|
71
|
-
|
|
72
|
-
for (let i = 0; i < vec3Buffer.vec3List!.maxLength; i++) {
|
|
73
|
-
vec3Buffer.vec3List!.create();
|
|
74
|
-
}
|
|
75
|
-
|
|
76
|
-
return vec3Buffer;
|
|
77
|
-
};
|
|
78
|
-
|
|
79
|
-
const clippedFace = /*@__PURE__*/ Face.create({
|
|
80
|
-
numVertices: 0,
|
|
81
|
-
buffer: { pool: new Vec3.Pool(32), maxLength: 32 },
|
|
82
|
-
});
|
|
83
|
-
|
|
84
|
-
const bodyAInverseOrientation = /*@__PURE__*/ Quat.create();
|
|
85
|
-
|
|
86
|
-
const edgeVertex1 = /*@__PURE__*/ Vec3.create();
|
|
87
|
-
const edgeVertex2 = /*@__PURE__*/ Vec3.create();
|
|
88
|
-
const planeOrigin = /*@__PURE__*/ Vec3.create();
|
|
89
|
-
const planeNormal = /*@__PURE__*/ Vec3.create();
|
|
90
|
-
const firstEdge = /*@__PURE__*/ Vec3.create();
|
|
91
|
-
const secondEdge = /*@__PURE__*/ Vec3.create();
|
|
92
|
-
const perpendicular = /*@__PURE__*/ Vec3.create();
|
|
93
|
-
const p2 = /*@__PURE__*/ Vec3.create();
|
|
94
|
-
const vectorAB = /*@__PURE__*/ Vec3.create();
|
|
95
|
-
const p1 = /*@__PURE__*/ Vec3.create();
|
|
96
|
-
|
|
97
|
-
const projected = /*@__PURE__*/ Vec3Buffer.create({
|
|
98
|
-
numItems: 0,
|
|
99
|
-
vec3List: { pool: new Vec3.Pool(64), maxLength: 64 },
|
|
100
|
-
});
|
|
101
|
-
|
|
102
|
-
const penetrationDepthSq = /*@__PURE__*/ Vec3Buffer.create({
|
|
103
|
-
numItems: 0,
|
|
104
|
-
vec3List: { pool: new Vec3.Pool(64), maxLength: 64 },
|
|
105
|
-
});
|
|
106
|
-
|
|
107
|
-
const pointsToKeepOnA = /*@__PURE__*/ Vec3Buffer.create({
|
|
108
|
-
numItems: 0,
|
|
109
|
-
vec3List: { pool: new Vec3.Pool(4), maxLength: 4 },
|
|
110
|
-
});
|
|
111
|
-
|
|
112
|
-
const pointsToKeepOnB = /*@__PURE__*/ Vec3Buffer.create({
|
|
113
|
-
numItems: 0,
|
|
114
|
-
vec3List: { pool: new Vec3.Pool(4), maxLength: 4 },
|
|
115
|
-
});
|
|
116
|
-
|
|
117
|
-
const v1 = /*@__PURE__*/ Vec3.create();
|
|
118
|
-
const projectedPoint = /*@__PURE__*/ Vec3.create();
|
|
119
|
-
const v2 = /*@__PURE__*/ Vec3.create();
|
|
120
|
-
const vector12 = /*@__PURE__*/ Vec3.create();
|
|
121
|
-
const currentProjectedPoint = /*@__PURE__*/ Vec3.create();
|
|
122
|
-
const point1v = /*@__PURE__*/ Vec3.create();
|
|
123
|
-
const differenceVector = /*@__PURE__*/ Vec3.create();
|
|
124
|
-
const point2v = /*@__PURE__*/ Vec3.create();
|
|
125
|
-
const perp = /*@__PURE__*/ Vec3.create();
|
|
126
|
-
const vectorPoint1vToPoint2v = /*@__PURE__*/ Vec3.create();
|
|
127
|
-
const tempPointA = /*@__PURE__*/ Vec3.create();
|
|
128
|
-
const tempPointB = /*@__PURE__*/ Vec3.create();
|
|
129
|
-
|
|
130
|
-
const inverseOrientationA = /*@__PURE__*/ Quat.create();
|
|
131
|
-
const translationAB = /*@__PURE__*/ Vec3.create();
|
|
132
|
-
const rotationAB = /*@__PURE__*/ Quat.create();
|
|
133
|
-
const localToWorldA = /*@__PURE__*/ Mat4.create();
|
|
134
|
-
const localToWorldB = /*@__PURE__*/ Mat4.create();
|
|
135
|
-
const localPointA = /*@__PURE__*/ Vec3.create();
|
|
136
|
-
const localPointB = /*@__PURE__*/ Vec3.create();
|
|
137
|
-
const worldPointA = /*@__PURE__*/ Vec3.create();
|
|
138
|
-
const worldPointB = /*@__PURE__*/ Vec3.create();
|
|
139
|
-
const vectorBA = /*@__PURE__*/ Vec3.create();
|
|
140
|
-
|
|
141
|
-
const tempManifoldPool = /*@__PURE__*/ new ContactManifoldPool(1, 4);
|
|
142
|
-
const tempManifold = /*@__PURE__*/ tempManifoldPool.createContactManifold({
|
|
143
|
-
bodyA: null,
|
|
144
|
-
bodyB: null,
|
|
145
|
-
subShapeIdA: 0,
|
|
146
|
-
subShapeIdB: 0,
|
|
147
|
-
nextContactManifold: null,
|
|
148
|
-
|
|
149
|
-
baseTranslation: { x: 0, y: 0, z: 0 },
|
|
150
|
-
firstWorldSpaceNormal: { x: 0, y: 0, z: 0 },
|
|
151
|
-
worldSpaceNormal: { x: 0, y: 0, z: 0 },
|
|
152
|
-
penetrationDepth: 0,
|
|
153
|
-
numContacts: 0,
|
|
154
|
-
});
|
|
155
|
-
|
|
156
|
-
export class ContactManifoldModule {
|
|
157
|
-
manifoldCaches: [ManifoldCache, ManifoldCache];
|
|
158
|
-
currentManifoldCacheIndex: number;
|
|
159
|
-
|
|
160
|
-
constructor(options?: Partial<ManifoldCacheOptions>) {
|
|
161
|
-
this.manifoldCaches = [new ManifoldCache(options), new ManifoldCache(options)];
|
|
162
|
-
this.currentManifoldCacheIndex = 0;
|
|
163
|
-
}
|
|
164
|
-
|
|
165
|
-
get currentManifoldCache() {
|
|
166
|
-
return this.manifoldCaches[this.currentManifoldCacheIndex];
|
|
167
|
-
}
|
|
168
|
-
|
|
169
|
-
get previousManifoldCache() {
|
|
170
|
-
return this.manifoldCaches[1 - this.currentManifoldCacheIndex];
|
|
171
|
-
}
|
|
172
|
-
|
|
173
|
-
toArray(array: NumberArray, startOffset: number): number {
|
|
174
|
-
array[startOffset++] = this.currentManifoldCacheIndex;
|
|
175
|
-
startOffset = this.manifoldCaches[0].toArray(array, startOffset);
|
|
176
|
-
startOffset = this.manifoldCaches[1].toArray(array, startOffset);
|
|
177
|
-
return startOffset;
|
|
178
|
-
}
|
|
179
|
-
|
|
180
|
-
fromArray(
|
|
181
|
-
array: NumberArray,
|
|
182
|
-
references: {
|
|
183
|
-
body: PoolClass<Body>;
|
|
184
|
-
},
|
|
185
|
-
startOffset: number
|
|
186
|
-
): number {
|
|
187
|
-
this.currentManifoldCacheIndex = array[startOffset++];
|
|
188
|
-
startOffset = this.manifoldCaches[0].fromArray(array, references, startOffset);
|
|
189
|
-
startOffset = this.manifoldCaches[1].fromArray(array, references, startOffset);
|
|
190
|
-
return startOffset;
|
|
191
|
-
}
|
|
192
|
-
|
|
193
|
-
addContactPair(bodyA: Body, bodyB: Body): ContactPair {
|
|
194
|
-
const pair = ContactPair.create(
|
|
195
|
-
{
|
|
196
|
-
bodyA: bodyA,
|
|
197
|
-
bodyB: bodyB,
|
|
198
|
-
translationAB: { x: 0, y: 0, z: 0 },
|
|
199
|
-
rotationAB: { x: 0, y: 0, z: 0, w: 1 },
|
|
200
|
-
},
|
|
201
|
-
this.currentManifoldCache.pairs
|
|
202
|
-
);
|
|
203
|
-
|
|
204
|
-
// add the pair to the current manifold cache pair map
|
|
205
|
-
this.currentManifoldCache.pairMap.set(pair.key, pair);
|
|
206
|
-
|
|
207
|
-
// get the quat to transform from World to A space
|
|
208
|
-
|
|
209
|
-
bodyAInverseOrientation.conjugateQuat(bodyA.orientation); // we can use the conjugate as the inverse since the quat is normalized
|
|
210
|
-
|
|
211
|
-
// get the relative translation from A to B in world space
|
|
212
|
-
pair.translationAB.subtractVectors(bodyB.computedCenterOfMassPosition, bodyA.computedCenterOfMassPosition);
|
|
213
|
-
|
|
214
|
-
// transform it to A space
|
|
215
|
-
pair.translationAB.transformByQuat(bodyAInverseOrientation);
|
|
216
|
-
|
|
217
|
-
// get the relative rotation from A to B in world space
|
|
218
|
-
pair.rotationAB.multiplyQuats(bodyAInverseOrientation, bodyB.orientation);
|
|
219
|
-
|
|
220
|
-
return pair;
|
|
221
|
-
}
|
|
222
|
-
|
|
223
|
-
pruneContactPoints(inPenetrationAxis: Vec3, ioManifold: ContactManifold) {
|
|
224
|
-
// console.log("\nContactManifoldModule.pruneContactPoints");
|
|
225
|
-
// #v-ifdef DEV
|
|
226
|
-
// assert that manifold has more than 4 contact points
|
|
227
|
-
if (ioManifold.numContacts <= 4) {
|
|
228
|
-
throw new Error("ContactManifoldModule.pruneContactPoints: manifold must have more than 4 contact points");
|
|
229
|
-
}
|
|
230
|
-
// #v-endif
|
|
231
|
-
|
|
232
|
-
// #v-ifdef DEV
|
|
233
|
-
// assert that penetration axis is normalized
|
|
234
|
-
if (inPenetrationAxis.isNormalized() === false) {
|
|
235
|
-
throw new Error("ContactManifoldModule.pruneContactPoints: penetrationAxis must be normalized");
|
|
236
|
-
}
|
|
237
|
-
// #v-endif
|
|
238
|
-
|
|
239
|
-
// We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep
|
|
240
|
-
// Neither of those two terms should ever become zero, so we clamp against this minimum value
|
|
241
|
-
const cMinDistanceSq = 1.0e-6; // 1 mm
|
|
242
|
-
|
|
243
|
-
projected.clear();
|
|
244
|
-
penetrationDepthSq.clear();
|
|
245
|
-
|
|
246
|
-
for (let i = 0; i < ioManifold.numContacts; i++) {
|
|
247
|
-
// Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1
|
|
248
|
-
// (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin)
|
|
249
|
-
|
|
250
|
-
ioManifold.getContactPointA(v1, i);
|
|
251
|
-
|
|
252
|
-
projectedPoint.addScaledToVector(v1, inPenetrationAxis, -v1.dot(inPenetrationAxis));
|
|
253
|
-
|
|
254
|
-
projected.pushVec3(projectedPoint);
|
|
255
|
-
|
|
256
|
-
// Calculate penetration depth^2 of each point and clamp against the minimal distance
|
|
257
|
-
|
|
258
|
-
ioManifold.getContactPointB(v2, i);
|
|
259
|
-
|
|
260
|
-
vector12.subtractVectors(v2, v1);
|
|
261
|
-
|
|
262
|
-
penetrationDepthSq.pushNumber(Math.max(cMinDistanceSq, vector12.squaredLength()));
|
|
263
|
-
}
|
|
264
|
-
|
|
265
|
-
// Find the point that is furthest away from the center of mass (its torque will have the biggest influence)
|
|
266
|
-
// and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this.
|
|
267
|
-
let point1 = 0;
|
|
268
|
-
|
|
269
|
-
projected.getVec3(currentProjectedPoint, 0);
|
|
270
|
-
|
|
271
|
-
let val = Math.max(cMinDistanceSq, currentProjectedPoint.squaredLength()) * penetrationDepthSq.getNumber(0);
|
|
272
|
-
|
|
273
|
-
for (let i = 0; i < projected.numItems; i++) {
|
|
274
|
-
projected.getVec3(currentProjectedPoint, i);
|
|
275
|
-
let v = Math.max(cMinDistanceSq, currentProjectedPoint.squaredLength()) * penetrationDepthSq.getNumber(i);
|
|
276
|
-
if (v > val) {
|
|
277
|
-
val = v;
|
|
278
|
-
point1 = i;
|
|
279
|
-
}
|
|
280
|
-
}
|
|
281
|
-
|
|
282
|
-
projected.getVec3(point1v, point1);
|
|
283
|
-
|
|
284
|
-
// Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic
|
|
285
|
-
// for deepest point as per above.
|
|
286
|
-
let point2 = -1;
|
|
287
|
-
val = -Infinity;
|
|
288
|
-
|
|
289
|
-
for (let i = 0; i < projected.numItems; i++) {
|
|
290
|
-
if (i !== point1) {
|
|
291
|
-
projected.getVec3(currentProjectedPoint, i);
|
|
292
|
-
|
|
293
|
-
differenceVector.subtractVectors(currentProjectedPoint, point1v);
|
|
294
|
-
let v = Math.max(cMinDistanceSq, differenceVector.squaredLength()) * penetrationDepthSq.getNumber(i);
|
|
295
|
-
|
|
296
|
-
if (v > val) {
|
|
297
|
-
val = v;
|
|
298
|
-
point2 = i;
|
|
299
|
-
}
|
|
300
|
-
}
|
|
301
|
-
}
|
|
302
|
-
|
|
303
|
-
// #v-ifdef DEV
|
|
304
|
-
// assert that point2 is not -1
|
|
305
|
-
if (point2 === -1) {
|
|
306
|
-
throw new Error("ContactManifoldModule.pruneContactPoints: point2 should not be -1");
|
|
307
|
-
}
|
|
308
|
-
// #v-endif
|
|
309
|
-
|
|
310
|
-
projected.getVec3(point2v, point2);
|
|
311
|
-
|
|
312
|
-
// Find furthest points on both sides of the line segment in order to maximize the area
|
|
313
|
-
let point3 = -1;
|
|
314
|
-
let point4 = -1;
|
|
315
|
-
let min_val = 0.0;
|
|
316
|
-
let max_val = 0.0;
|
|
317
|
-
|
|
318
|
-
vectorPoint1vToPoint2v.subtractVectors(point2v, point1v);
|
|
319
|
-
perp.crossVectors(vectorPoint1vToPoint2v, inPenetrationAxis);
|
|
320
|
-
|
|
321
|
-
for (let i = 0; i < projected.numItems; i++) {
|
|
322
|
-
if (i !== point1 && i !== point2) {
|
|
323
|
-
projected.getVec3(currentProjectedPoint, i);
|
|
324
|
-
|
|
325
|
-
differenceVector.subtractVectors(currentProjectedPoint, point1v);
|
|
326
|
-
const v = perp.dot(differenceVector);
|
|
327
|
-
|
|
328
|
-
if (v < min_val) {
|
|
329
|
-
min_val = v;
|
|
330
|
-
point3 = i;
|
|
331
|
-
} else if (v > max_val) {
|
|
332
|
-
max_val = v;
|
|
333
|
-
point4 = i;
|
|
334
|
-
}
|
|
335
|
-
}
|
|
336
|
-
}
|
|
337
|
-
|
|
338
|
-
pointsToKeepOnA.clear();
|
|
339
|
-
pointsToKeepOnB.clear();
|
|
340
|
-
|
|
341
|
-
ioManifold.getContactPointA(tempPointA, point1);
|
|
342
|
-
pointsToKeepOnA.pushVec3(tempPointA);
|
|
343
|
-
|
|
344
|
-
ioManifold.getContactPointB(tempPointB, point1);
|
|
345
|
-
pointsToKeepOnB.pushVec3(tempPointB);
|
|
346
|
-
|
|
347
|
-
if (point3 !== -1) {
|
|
348
|
-
ioManifold.getContactPointA(tempPointA, point3);
|
|
349
|
-
pointsToKeepOnA.pushVec3(tempPointA);
|
|
350
|
-
|
|
351
|
-
ioManifold.getContactPointB(tempPointB, point3);
|
|
352
|
-
pointsToKeepOnB.pushVec3(tempPointB);
|
|
353
|
-
}
|
|
354
|
-
|
|
355
|
-
ioManifold.getContactPointA(tempPointA, point2);
|
|
356
|
-
pointsToKeepOnA.pushVec3(tempPointA);
|
|
357
|
-
|
|
358
|
-
ioManifold.getContactPointB(tempPointB, point2);
|
|
359
|
-
pointsToKeepOnB.pushVec3(tempPointB);
|
|
360
|
-
|
|
361
|
-
if (point4 !== -1) {
|
|
362
|
-
ioManifold.getContactPointA(tempPointA, point4);
|
|
363
|
-
pointsToKeepOnA.pushVec3(tempPointA);
|
|
364
|
-
|
|
365
|
-
ioManifold.getContactPointB(tempPointB, point4);
|
|
366
|
-
pointsToKeepOnB.pushVec3(tempPointB);
|
|
367
|
-
}
|
|
368
|
-
|
|
369
|
-
// set the points on the manifold
|
|
370
|
-
ioManifold.clear();
|
|
371
|
-
|
|
372
|
-
for (let i = 0; i < pointsToKeepOnA.numItems; i++) {
|
|
373
|
-
pointsToKeepOnA.getVec3(tempPointA, i);
|
|
374
|
-
pointsToKeepOnB.getVec3(tempPointB, i);
|
|
375
|
-
ioManifold.pushContactPoints(tempPointA, tempPointB);
|
|
376
|
-
}
|
|
377
|
-
}
|
|
378
|
-
|
|
379
|
-
getContactsFromCache(
|
|
380
|
-
bodyA: Body,
|
|
381
|
-
bodyB: Body,
|
|
382
|
-
contactConstraintModule: ContactConstraintModule,
|
|
383
|
-
sleepModule: SleepModule,
|
|
384
|
-
isWarmStartingEnabled: boolean,
|
|
385
|
-
timeStepSizeSeconds: number,
|
|
386
|
-
minVelocityForElasticContact: number
|
|
387
|
-
): [boolean, boolean] {
|
|
388
|
-
let pairHandled = false;
|
|
389
|
-
let constraintCreated = false;
|
|
390
|
-
|
|
391
|
-
// find the previous cached pair
|
|
392
|
-
const pairKey = createContactPairKey(bodyA, bodyB);
|
|
393
|
-
const previousPair = this.previousManifoldCache.pairMap.get(pairKey);
|
|
394
|
-
|
|
395
|
-
// exit if previous cached pair doesn't exist
|
|
396
|
-
if (previousPair === undefined) {
|
|
397
|
-
return [pairHandled, constraintCreated];
|
|
398
|
-
}
|
|
399
|
-
|
|
400
|
-
// get the current relative translation (AB) in A space
|
|
401
|
-
|
|
402
|
-
inverseOrientationA.conjugateQuat(bodyA.orientation); // we're able to conjugate instead of invert because it's a unit quaternion
|
|
403
|
-
|
|
404
|
-
translationAB.subtractVectors(bodyB.computedCenterOfMassPosition, bodyA.computedCenterOfMassPosition);
|
|
405
|
-
|
|
406
|
-
translationAB.transformVectorByQuat(translationAB, inverseOrientationA);
|
|
407
|
-
|
|
408
|
-
// compare with the previous relative translation (AB) in A space
|
|
409
|
-
const squaredDistance = translationAB.squaredDistance(previousPair.translationAB);
|
|
410
|
-
|
|
411
|
-
// exit if the relative translation has changed too much
|
|
412
|
-
if (squaredDistance > contactPairCacheMaxDeltaPositionSquared) {
|
|
413
|
-
return [pairHandled, constraintCreated];
|
|
414
|
-
}
|
|
415
|
-
|
|
416
|
-
// get the current relative orientation (AB) in A space
|
|
417
|
-
|
|
418
|
-
rotationAB.multiplyQuats(inverseOrientationA, bodyB.orientation);
|
|
419
|
-
|
|
420
|
-
// compare with the previous relative orientation (AB) in A space
|
|
421
|
-
const deltaAngle = Math.abs(rotationAB.dot(previousPair.rotationAB));
|
|
422
|
-
|
|
423
|
-
// exit if the relative orientation has changed too much
|
|
424
|
-
if (deltaAngle < contactPairCacheCosMaxDeltaRotationDiv2) {
|
|
425
|
-
return [pairHandled, constraintCreated];
|
|
426
|
-
}
|
|
427
|
-
|
|
428
|
-
// if we've reached here, the previous cache is valid
|
|
429
|
-
pairHandled = true;
|
|
430
|
-
|
|
431
|
-
// 1. create a new pair in the current cache and copy the previous cached pair data
|
|
432
|
-
|
|
433
|
-
const currentPair = ContactPair.create(
|
|
434
|
-
{
|
|
435
|
-
bodyA: bodyA,
|
|
436
|
-
bodyB: bodyB,
|
|
437
|
-
firstContactManifold: null, // will be set later
|
|
438
|
-
translationAB: translationAB,
|
|
439
|
-
rotationAB: rotationAB,
|
|
440
|
-
},
|
|
441
|
-
this.currentManifoldCache.pairs
|
|
442
|
-
) as ContactPair;
|
|
443
|
-
|
|
444
|
-
// copy the previous cached pair data
|
|
445
|
-
currentPair.copy(previousPair);
|
|
446
|
-
|
|
447
|
-
// add the pair to the pair map AFTER copying the data
|
|
448
|
-
this.currentManifoldCache.pairMap.set(currentPair.key, currentPair);
|
|
449
|
-
|
|
450
|
-
// exit if no contact manifolds to handle
|
|
451
|
-
if (!currentPair.firstContactManifold) {
|
|
452
|
-
return [pairHandled, constraintCreated];
|
|
453
|
-
}
|
|
454
|
-
|
|
455
|
-
// 2. create new manifolds and copy manifold data. care must be taken here as the manifold ids are different between the two caches!!
|
|
456
|
-
|
|
457
|
-
// reset the current contact pair's first manifold id
|
|
458
|
-
// TODO: replace or implement the manifold linked list
|
|
459
|
-
currentPair.firstContactManifold = null;
|
|
460
|
-
|
|
461
|
-
// get local to world transforms for each body
|
|
462
|
-
|
|
463
|
-
localToWorldA.fromRotationTranslation(bodyA.orientation, bodyA.computedCenterOfMassPosition);
|
|
464
|
-
localToWorldB.fromRotationTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
|
|
465
|
-
|
|
466
|
-
// walk over all manifolds in the previous cache and for each one, create a new manifold in the current cache with the same data (ids will be different)
|
|
467
|
-
// TODO: replace or implement the manifold linked list
|
|
468
|
-
let nextManifoldIdInPreviousCache = previousPair.firstContactManifold;
|
|
469
|
-
while (nextManifoldIdInPreviousCache !== null) {
|
|
470
|
-
// add a new contact manifold and manifold constraint
|
|
471
|
-
// convert the previous manifold from local space into a temp manifold in world space
|
|
472
|
-
// memcopy the previous manifold into the temp manifold
|
|
473
|
-
tempManifold.copy(nextManifoldIdInPreviousCache);
|
|
474
|
-
tempManifold.nextContactManifold = null;
|
|
475
|
-
|
|
476
|
-
// convert the normal from B space to world space
|
|
477
|
-
localToWorldB.multiply3x3(tempManifold.worldSpaceNormal, nextManifoldIdInPreviousCache.worldSpaceNormal);
|
|
478
|
-
tempManifold.worldSpaceNormal.normalize();
|
|
479
|
-
|
|
480
|
-
// estimate a new penetration depth
|
|
481
|
-
let penetrationDepth = -Infinity;
|
|
482
|
-
|
|
483
|
-
for (let i = 0; i < nextManifoldIdInPreviousCache.numContacts; i++) {
|
|
484
|
-
// get the contact point in local space
|
|
485
|
-
nextManifoldIdInPreviousCache.getContactPointA(localPointA, i);
|
|
486
|
-
nextManifoldIdInPreviousCache.getContactPointB(localPointB, i);
|
|
487
|
-
|
|
488
|
-
// convert the contact point to world space
|
|
489
|
-
worldPointA.transformVectorFromMat4(localPointA, localToWorldA);
|
|
490
|
-
worldPointB.transformVectorFromMat4(localPointB, localToWorldB);
|
|
491
|
-
|
|
492
|
-
// set the contact points in the temp manifold
|
|
493
|
-
tempManifold.setContactPointA(worldPointA, i);
|
|
494
|
-
tempManifold.setContactPointB(worldPointB, i);
|
|
495
|
-
|
|
496
|
-
// calculate the penetration depth
|
|
497
|
-
vectorBA.subtractVectors(worldPointA, worldPointB);
|
|
498
|
-
penetrationDepth = Math.max(penetrationDepth, vectorBA.dot(tempManifold.worldSpaceNormal));
|
|
499
|
-
|
|
500
|
-
// copy the lambdas
|
|
501
|
-
if (isWarmStartingEnabled) {
|
|
502
|
-
tempManifold.copyLambda(nextManifoldIdInPreviousCache, i);
|
|
503
|
-
}
|
|
504
|
-
}
|
|
505
|
-
|
|
506
|
-
tempManifold.penetrationDepth = penetrationDepth;
|
|
507
|
-
|
|
508
|
-
contactConstraintModule.addContactConstraints(
|
|
509
|
-
bodyA,
|
|
510
|
-
bodyB,
|
|
511
|
-
tempManifold,
|
|
512
|
-
timeStepSizeSeconds,
|
|
513
|
-
isWarmStartingEnabled,
|
|
514
|
-
minVelocityForElasticContact
|
|
515
|
-
);
|
|
516
|
-
constraintCreated = true;
|
|
517
|
-
|
|
518
|
-
// get the next manifold id in the previous cache
|
|
519
|
-
// TODO: replace or implement the manifold linked list
|
|
520
|
-
nextManifoldIdInPreviousCache = nextManifoldIdInPreviousCache.nextContactManifold;
|
|
521
|
-
}
|
|
522
|
-
|
|
523
|
-
return [pairHandled, constraintCreated];
|
|
524
|
-
}
|
|
525
|
-
|
|
526
|
-
manifoldBetweenTwoFaces(
|
|
527
|
-
pointA: Vec3,
|
|
528
|
-
pointB: Vec3,
|
|
529
|
-
penetrationAxis: Vec3,
|
|
530
|
-
maxContactDistanceSquared: number,
|
|
531
|
-
faceA: Face,
|
|
532
|
-
faceB: Face,
|
|
533
|
-
outManifold: ContactManifold
|
|
534
|
-
): void {
|
|
535
|
-
// remember size before adding new points, to check if we added any
|
|
536
|
-
let originalNumberOfContactPoints = outManifold.numContacts;
|
|
537
|
-
|
|
538
|
-
// case: both faces are polygons
|
|
539
|
-
if (faceA.numVertices >= 2 && faceB.numVertices >= 3) {
|
|
540
|
-
// clip polygon of face B against face A
|
|
541
|
-
clippedFace.clear();
|
|
542
|
-
|
|
543
|
-
// case: face A is a polygon
|
|
544
|
-
if (faceA.numVertices >= 3) {
|
|
545
|
-
clipPolyVsPoly(clippedFace, faceB, faceA, penetrationAxis);
|
|
546
|
-
}
|
|
547
|
-
// case: face A is an edge
|
|
548
|
-
else if (faceA.numVertices === 2) {
|
|
549
|
-
faceA.getVertex(edgeVertex1, 0);
|
|
550
|
-
|
|
551
|
-
faceA.getVertex(edgeVertex2, 1);
|
|
552
|
-
|
|
553
|
-
clipPolyVsEdge(clippedFace, faceB, edgeVertex1, edgeVertex2, penetrationAxis);
|
|
554
|
-
}
|
|
555
|
-
|
|
556
|
-
// project the points back onto the plane of face A and only keep those that are behind the plane
|
|
557
|
-
|
|
558
|
-
planeOrigin.zero();
|
|
559
|
-
|
|
560
|
-
planeNormal.zero();
|
|
561
|
-
|
|
562
|
-
firstEdge.zero();
|
|
563
|
-
|
|
564
|
-
secondEdge.zero();
|
|
565
|
-
|
|
566
|
-
// get first vertex
|
|
567
|
-
faceA.getVertex(planeOrigin, 0);
|
|
568
|
-
|
|
569
|
-
// get first edge
|
|
570
|
-
faceA.getVertex(firstEdge, 1);
|
|
571
|
-
firstEdge.subtractVectors(firstEdge, planeOrigin);
|
|
572
|
-
|
|
573
|
-
// if (faceA.vertexCapacity >= 3) {
|
|
574
|
-
if (faceA.numVertices >= 3) {
|
|
575
|
-
// get second edge
|
|
576
|
-
faceA.getVertex(secondEdge, 2);
|
|
577
|
-
secondEdge.subtractVectors(secondEdge, planeOrigin);
|
|
578
|
-
|
|
579
|
-
// three vertices, so we can calculate the normal
|
|
580
|
-
planeNormal.crossVectors(firstEdge, secondEdge);
|
|
581
|
-
} else {
|
|
582
|
-
// two vertices, first find a perpendicular to the first edge and penetration axis and then use the perpendicular with the edge to get the normal
|
|
583
|
-
|
|
584
|
-
perpendicular.crossVectors(firstEdge, penetrationAxis);
|
|
585
|
-
planeNormal.crossVectors(perpendicular, firstEdge);
|
|
586
|
-
}
|
|
587
|
-
|
|
588
|
-
negatedPenetrationAxis.negateVector(penetrationAxis);
|
|
589
|
-
// check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points
|
|
590
|
-
const penetrationAxisDotPlaneNormal = penetrationAxis.dot(planeNormal);
|
|
591
|
-
if (penetrationAxisDotPlaneNormal != 0.0) {
|
|
592
|
-
const penetrationAxisLength = penetrationAxis.length();
|
|
593
|
-
|
|
594
|
-
// discard the points of faces that are too far away to collide
|
|
595
|
-
// iterate over clipped face vertices
|
|
596
|
-
for (let i = 0; i < clippedFace.numVertices; i++) {
|
|
597
|
-
clippedFace.getVertex(p2, i);
|
|
598
|
-
vectorAB.subtractVectors(p2, planeOrigin);
|
|
599
|
-
const distance = vectorAB.dot(planeNormal) / penetrationAxisDotPlaneNormal;
|
|
600
|
-
if (distance * penetrationAxisLength < maxContactDistanceSquared) {
|
|
601
|
-
p1.addScaledToVector(p2, negatedPenetrationAxis, distance);
|
|
602
|
-
|
|
603
|
-
// add the contact points
|
|
604
|
-
outManifold.pushContactPoints(p1, p2);
|
|
605
|
-
}
|
|
606
|
-
}
|
|
607
|
-
}
|
|
608
|
-
}
|
|
609
|
-
|
|
610
|
-
// case: use contact points themselves if no new points were added
|
|
611
|
-
if (originalNumberOfContactPoints === outManifold.numContacts) {
|
|
612
|
-
outManifold.pushContactPoints(pointA, pointB);
|
|
613
|
-
}
|
|
614
|
-
}
|
|
615
|
-
|
|
616
|
-
flipCache(): void {
|
|
617
|
-
// switch the current cache with the previous cache
|
|
618
|
-
this.currentManifoldCacheIndex ^= 1;
|
|
619
|
-
|
|
620
|
-
// clear the current cache
|
|
621
|
-
this.currentManifoldCache.clear();
|
|
622
|
-
}
|
|
623
|
-
}
|