@perplexdotgg/bounce 1.0.0

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.
Files changed (140) hide show
  1. package/LICENSE +21 -0
  2. package/README.md +30 -0
  3. package/package.json +32 -0
  4. package/src/builders/ConvexHullBuilder.ts +437 -0
  5. package/src/builders/ConvexHullBuilder2d.ts +344 -0
  6. package/src/builders/ConvexHullBuilder3d.ts +1689 -0
  7. package/src/builders/HeightMapBuilder.ts +414 -0
  8. package/src/builders/TriangleMeshBuilder.ts +92 -0
  9. package/src/collision/CastShapesModule.ts +184 -0
  10. package/src/collision/CollideShapesModule.ts +152 -0
  11. package/src/collision/HeightMapCaster.ts +38 -0
  12. package/src/collision/HeightMapCollider.ts +33 -0
  13. package/src/collision/TriangleCaster.ts +249 -0
  14. package/src/collision/TriangleCollider.ts +308 -0
  15. package/src/collision/TriangleCollider2.ts +379 -0
  16. package/src/collision/activeEdge.ts +146 -0
  17. package/src/collision/cast/cast.ts +139 -0
  18. package/src/collision/cast/castCompoundVsCompound.ts +59 -0
  19. package/src/collision/cast/castCompoundVsConvex.ts +116 -0
  20. package/src/collision/cast/castConvexVsCompound.ts +123 -0
  21. package/src/collision/cast/castConvexVsConvex.ts +213 -0
  22. package/src/collision/cast/castConvexVsHeightMap.ts +73 -0
  23. package/src/collision/cast/castConvexVsTriangleMesh.ts +56 -0
  24. package/src/collision/cast/castRayVsCompound.ts +44 -0
  25. package/src/collision/cast/castRayVsConvex.ts +45 -0
  26. package/src/collision/cast/castRayVsHeightMap.ts +58 -0
  27. package/src/collision/cast/castRayVsTriangleMesh.ts +58 -0
  28. package/src/collision/closestPoints/closestPoints.ts +23 -0
  29. package/src/collision/closestPoints/computeBarycentricCoordinates2d.ts +32 -0
  30. package/src/collision/closestPoints/computeBarycentricCoordinates3d.ts +81 -0
  31. package/src/collision/closestPoints/computeClosestPointOnLine.ts +30 -0
  32. package/src/collision/closestPoints/computeClosestPointOnTetrahedron.ts +96 -0
  33. package/src/collision/closestPoints/computeClosestPointOnTriangle.ts +195 -0
  34. package/src/collision/closestPoints/isOriginOutsideOfPlane.ts +25 -0
  35. package/src/collision/closestPoints/isOriginOutsideOfTrianglePlanes.ts +72 -0
  36. package/src/collision/collide/collide.ts +146 -0
  37. package/src/collision/collide/collideCompoundVsCompound.ts +60 -0
  38. package/src/collision/collide/collideCompoundVsConvex.ts +59 -0
  39. package/src/collision/collide/collideCompoundVsHeightMap.ts +73 -0
  40. package/src/collision/collide/collideCompoundVsTriangleMesh.ts +56 -0
  41. package/src/collision/collide/collideConvexVsCompound.ts +57 -0
  42. package/src/collision/collide/collideConvexVsConvex.ts +225 -0
  43. package/src/collision/collide/collideConvexVsConvexImp.ts +236 -0
  44. package/src/collision/collide/collideConvexVsHeightMap.ts +53 -0
  45. package/src/collision/collide/collideConvexVsTriangleMesh.ts +58 -0
  46. package/src/collision/collide/collideHeightMapVsCompound.ts +69 -0
  47. package/src/collision/collide/collideHeightMapVsConvex.ts +53 -0
  48. package/src/collision/collide/collideSphereVsSphere.ts +81 -0
  49. package/src/collision/collide/collideTriangleMeshVsCompound.ts +58 -0
  50. package/src/collision/collide/collideTriangleMeshVsConvex.ts +58 -0
  51. package/src/collision/epa/EpaConvexHullBuilder.ts +397 -0
  52. package/src/collision/epa/StaticArray.ts +154 -0
  53. package/src/collision/epa/TriangleFactory.ts +32 -0
  54. package/src/collision/epa/arrays.ts +99 -0
  55. package/src/collision/epa/binaryHeap.ts +82 -0
  56. package/src/collision/epa/structs.ts +227 -0
  57. package/src/collision/gjk/GjkModule.ts +864 -0
  58. package/src/collision/gjk/PenetrationDepthModule.ts +493 -0
  59. package/src/collision/gjk/SupportPoints.ts +50 -0
  60. package/src/collision/imp/MinkowskiDifference.ts +36 -0
  61. package/src/collision/imp/computeExploredDistanceLowerUpperBound.ts +40 -0
  62. package/src/collision/imp/finalizeImpResult.ts +69 -0
  63. package/src/collision/imp/findContactImp.ts +196 -0
  64. package/src/collision/imp/imp.ts +28 -0
  65. package/src/collision/imp/incrementalMinimumDistanceExploreDirection.ts +207 -0
  66. package/src/collision/mpr/findPortal.ts +152 -0
  67. package/src/collision/mpr/mpr.ts +29 -0
  68. package/src/collision/mpr/updatePortal.ts +52 -0
  69. package/src/constraints/BaseConstraint.ts +50 -0
  70. package/src/constraints/ConstraintOptions.ts +22 -0
  71. package/src/constraints/ConstraintSolver.ts +119 -0
  72. package/src/constraints/DistanceConstraint.ts +229 -0
  73. package/src/constraints/FixedConstraint.ts +203 -0
  74. package/src/constraints/HingeConstraint.ts +460 -0
  75. package/src/constraints/PointConstraint.ts +108 -0
  76. package/src/constraints/components/AngleComponent.ts +226 -0
  77. package/src/constraints/components/AxisComponent.ts +263 -0
  78. package/src/constraints/components/HingeComponent.ts +215 -0
  79. package/src/constraints/components/Motor.ts +36 -0
  80. package/src/constraints/components/PointConstraintComponent.ts +179 -0
  81. package/src/constraints/components/RotationEulerComponent.ts +139 -0
  82. package/src/constraints/components/Spring.ts +30 -0
  83. package/src/constraints/components/SpringComponent.ts +71 -0
  84. package/src/constraints/types.ts +6 -0
  85. package/src/helpers.ts +147 -0
  86. package/src/index.ts +50 -0
  87. package/src/math/BasicTransform.ts +19 -0
  88. package/src/math/NumberValue.ts +13 -0
  89. package/src/math/isometry.ts +64 -0
  90. package/src/math/mat3.ts +529 -0
  91. package/src/math/mat4.ts +588 -0
  92. package/src/math/quat.ts +193 -0
  93. package/src/math/scalar.ts +81 -0
  94. package/src/math/tensor.ts +17 -0
  95. package/src/math/vec3.ts +589 -0
  96. package/src/math/vec4.ts +10 -0
  97. package/src/physics/Body.ts +581 -0
  98. package/src/physics/CollisionFilter.ts +52 -0
  99. package/src/physics/SleepModule.ts +163 -0
  100. package/src/physics/broadphase/BodyPairsModule.ts +363 -0
  101. package/src/physics/broadphase/BvhModule.ts +237 -0
  102. package/src/physics/broadphase/BvhTree.ts +803 -0
  103. package/src/physics/broadphase/ConstraintPairsModule.ts +385 -0
  104. package/src/physics/broadphase/TriangleMeshBvhTree.ts +379 -0
  105. package/src/physics/manifold/ContactManifold.ts +227 -0
  106. package/src/physics/manifold/ContactManifoldModule.ts +623 -0
  107. package/src/physics/manifold/Face.ts +119 -0
  108. package/src/physics/manifold/ManifoldCache.ts +116 -0
  109. package/src/physics/manifold/clipping/clipPolyVsEdge.ts +131 -0
  110. package/src/physics/manifold/clipping/clipPolyVsPlane.ts +73 -0
  111. package/src/physics/manifold/clipping/clipPolyVsPoly.ts +72 -0
  112. package/src/physics/narrowphase/CollideBodiesModule.ts +755 -0
  113. package/src/physics/solver/ContactConstraintModule.ts +659 -0
  114. package/src/physics/solver/ManifoldConstraint.ts +420 -0
  115. package/src/physics/solver/estimateCollisionResponse.ts +146 -0
  116. package/src/shape/Aabb.ts +400 -0
  117. package/src/shape/Box.ts +231 -0
  118. package/src/shape/Capsule.ts +332 -0
  119. package/src/shape/CompoundShape.ts +288 -0
  120. package/src/shape/Convex.ts +130 -0
  121. package/src/shape/ConvexHull.ts +423 -0
  122. package/src/shape/Cylinder.ts +313 -0
  123. package/src/shape/HeightMap.ts +511 -0
  124. package/src/shape/Line.ts +14 -0
  125. package/src/shape/Plane.ts +116 -0
  126. package/src/shape/Ray.ts +81 -0
  127. package/src/shape/Segment.ts +25 -0
  128. package/src/shape/Shape.ts +77 -0
  129. package/src/shape/Sphere.ts +181 -0
  130. package/src/shape/TransformedShape.ts +51 -0
  131. package/src/shape/Triangle.ts +122 -0
  132. package/src/shape/TriangleMesh.ts +186 -0
  133. package/src/types.ts +1 -0
  134. package/src/world.ts +1335 -0
  135. package/tests/BodyPairsModule.test.ts +71 -0
  136. package/tests/BvhTree.test.ts +406 -0
  137. package/tests/test.md +642 -0
  138. package/tests/vec3.test.ts +12 -0
  139. package/tsconfig.json +20 -0
  140. package/vite.config.js +40 -0
@@ -0,0 +1,73 @@
1
+ import { Isometry } from "../../math/isometry";
2
+ import { Vec3 } from "../../math/vec3";
3
+ import { Aabb } from "../../shape/Aabb";
4
+ import { CompoundShape } from "../../shape/CompoundShape";
5
+ import { HeightMap } from "../../shape/HeightMap";
6
+ import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
7
+ import { BackFaceMode, CollisionCollector, CollisionSettings } from "./collide";
8
+ import { HeightMapCollider } from "../HeightMapCollider";
9
+ import { Body } from "../../physics/Body";
10
+ import { BasicTransform, ConvexShape } from "../..";
11
+
12
+ const isometryWorldToB = /*@__PURE__*/ Isometry.create();
13
+ const isometryAToB = /*@__PURE__*/ Isometry.create();
14
+ const shapeABoundsInBSpace = /*@__PURE__*/ Aabb.create();
15
+
16
+ const transformSubShapeToA = /*@__PURE__*/ Isometry.create();
17
+ const transformSubShapeToWorld = /*@__PURE__*/ Isometry.create();
18
+
19
+ const heightMapCollider = /*@__PURE__*/ new HeightMapCollider();
20
+
21
+ export function collideCompoundVsHeightMap(
22
+ penetrationDepthModule: PenetrationDepthModule,
23
+ collector: CollisionCollector,
24
+ shapeA: CompoundShape,
25
+ shapeB: HeightMap,
26
+ isometryA: Isometry,
27
+ isometryB: Isometry,
28
+ settings: CollisionSettings,
29
+ bodyA: Body | null,
30
+ bodyB: Body | null,
31
+ initialDirection?: Vec3,
32
+ subShapeIdA: number = 0,
33
+ subShapeIdB: number = 0
34
+ ) {
35
+ // TODO: don't hardcode this option
36
+ settings.backFaceMode = BackFaceMode.CollideWithBackFaces;
37
+
38
+ let index = 0;
39
+ for (const shapeObj of shapeA.shapes) {
40
+ const shape = shapeObj.shape as ConvexShape;
41
+ const transform = shapeObj.transform as BasicTransform;
42
+ const translation = transform.position;
43
+ const rotation = transform.rotation;
44
+
45
+ // set the isometries
46
+ transformSubShapeToA.fromRotationAndTranslation(rotation, translation);
47
+ transformSubShapeToWorld.matrix.multiplyMatrices(isometryA.matrix, transformSubShapeToA.matrix);
48
+
49
+ heightMapCollider.triangleCollider.initialize(
50
+ penetrationDepthModule,
51
+ shape,
52
+ transformSubShapeToWorld,
53
+ isometryB,
54
+ settings,
55
+ collector,
56
+ index,
57
+ bodyA,
58
+ bodyB
59
+ );
60
+
61
+ isometryWorldToB.matrix.invertMatrix(isometryB.matrix);
62
+
63
+ isometryAToB.matrix.multiplyMatrices(transformSubShapeToWorld.matrix, isometryWorldToB.matrix);
64
+
65
+ shapeABoundsInBSpace.copy(shape.computedAabb);
66
+ shapeABoundsInBSpace.transform(isometryAToB);
67
+ shapeABoundsInBSpace.expand(settings.maxSeparation);
68
+ heightMapCollider.initialize(shapeABoundsInBSpace, bodyA, bodyB);
69
+ shapeB.walkHeightMap(heightMapCollider);
70
+
71
+ index++;
72
+ }
73
+ }
@@ -0,0 +1,56 @@
1
+ import { Isometry } from "../../math/isometry";
2
+ import { Vec3 } from "../../math/vec3";
3
+ import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
4
+ import { CollisionCollector, CollisionSettings } from "./collide";
5
+ import { CompoundShape } from "../../shape/CompoundShape";
6
+ import { TriangleMesh } from "../../shape/TriangleMesh";
7
+ import { collideConvexVsTriangleMesh } from "./collideConvexVsTriangleMesh";
8
+ import { Body } from "../../physics/Body";
9
+ import { ConvexShape } from "../..";
10
+
11
+ const transformSubShapeToA = /*@__PURE__*/ Isometry.create();
12
+ const transformSubShapeToWorld = /*@__PURE__*/ Isometry.create();
13
+
14
+ // the results are in world space
15
+ export function collideCompoundVsTriangleMesh(
16
+ penetrationDepthModule: PenetrationDepthModule,
17
+ collector: CollisionCollector,
18
+ shapeA: CompoundShape,
19
+ shapeB: TriangleMesh,
20
+ isometryA: Isometry,
21
+ isometryB: Isometry,
22
+ settings: CollisionSettings,
23
+ bodyA: Body | null,
24
+ bodyB: Body | null,
25
+ initialDirection?: Vec3,
26
+ subShapeIdA: number = 0,
27
+ subShapeIdB: number = 0
28
+ ): void {
29
+ let index = 0;
30
+ for (const { shape, transform } of shapeA.shapes) {
31
+ const translation = transform.position;
32
+ const rotation = transform.rotation;
33
+
34
+ // set the isometries
35
+ transformSubShapeToA.fromRotationAndTranslation(rotation, translation);
36
+ transformSubShapeToWorld.matrix.multiplyMatrices(isometryA.matrix, transformSubShapeToA.matrix);
37
+
38
+ // collide the shapes. TODO: this should be convex vs convex for now, but may be expanded in the future
39
+ collideConvexVsTriangleMesh(
40
+ penetrationDepthModule,
41
+ collector,
42
+ shape as ConvexShape,
43
+ shapeB,
44
+ transformSubShapeToWorld,
45
+ isometryB,
46
+ settings,
47
+ bodyA,
48
+ bodyB,
49
+ initialDirection,
50
+ index,
51
+ 0
52
+ );
53
+
54
+ index++;
55
+ }
56
+ }
@@ -0,0 +1,57 @@
1
+ import { Isometry } from "../../math/isometry";
2
+ import { Vec3 } from "../../math/vec3";
3
+ import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
4
+ import { CollisionCollector, CollisionSettings } from "./collide";
5
+ import { CompoundShape } from "../../shape/CompoundShape";
6
+ import { collideConvexVsConvex } from "./collideConvexVsConvex";
7
+ import { Body } from "../../physics/Body";
8
+ import { ConvexShape } from "../../shape/Shape";
9
+
10
+ const transformSubShapeBToB = /*@__PURE__*/ Isometry.create();
11
+ const transformSubShapeBToWorld = /*@__PURE__*/ Isometry.create();
12
+
13
+ // the results are in world space
14
+ export function collideConvexVsCompound(
15
+ penetrationDepthModule: PenetrationDepthModule,
16
+ collector: CollisionCollector,
17
+ shapeA: ConvexShape,
18
+ shapeB: CompoundShape,
19
+ isometryA: Isometry,
20
+ isometryB: Isometry,
21
+ settings: CollisionSettings,
22
+ bodyA: Body | null,
23
+ bodyB: Body | null,
24
+ initialDirection?: Vec3,
25
+ subShapeIdA: number = 0,
26
+ subShapeIdB: number = 0
27
+ ): void {
28
+ // throw new Error("Not implemented");
29
+
30
+ let index = 0;
31
+ for (const { shape: subShapeB, transform } of shapeB.shapes) {
32
+ const translation = transform.position;
33
+ const rotation = transform.rotation;
34
+
35
+ // set the isometries
36
+ transformSubShapeBToB.fromRotationAndTranslation(rotation, translation);
37
+ transformSubShapeBToWorld.matrix.multiplyMatrices(isometryB.matrix, transformSubShapeBToB.matrix);
38
+
39
+ // collide the shapes. TODO: this should be convex vs convex for now, but may be expanded in the future
40
+ collideConvexVsConvex(
41
+ penetrationDepthModule,
42
+ collector,
43
+ shapeA,
44
+ subShapeB as ConvexShape,
45
+ isometryA,
46
+ transformSubShapeBToWorld,
47
+ settings,
48
+ bodyA,
49
+ bodyB,
50
+ initialDirection,
51
+ 0,
52
+ index
53
+ );
54
+
55
+ index++;
56
+ }
57
+ }
@@ -0,0 +1,225 @@
1
+ import { Isometry } from "../../math/isometry";
2
+ import { Vec3 } from "../../math/vec3";
3
+ import { Face } from "../../physics/manifold/Face";
4
+ import { PenetrationDepth, PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
5
+ import { CollisionCollector, CollisionResult, CollisionSettings, CollisionStatus } from "./collide";
6
+ import {
7
+ ConvexRadiusObject,
8
+ ConvexWithcomputeSupportShape,
9
+ SupportMode,
10
+ TransformedConvexShape,
11
+ } from "../../shape/Convex";
12
+ import { Body } from "../../physics/Body";
13
+
14
+ const result = CollisionResult.create({
15
+ faceA: {
16
+ numVertices: 0,
17
+ buffer: { pool: new Vec3.Pool(32), maxLength: 32 },
18
+ },
19
+ faceB: {
20
+ numVertices: 0,
21
+ buffer: { pool: new Vec3.Pool(32), maxLength: 32 },
22
+ },
23
+ });
24
+
25
+ const positionA = /*@__PURE__*/ Vec3.create();
26
+ const positionB = /*@__PURE__*/ Vec3.create();
27
+ const searchDirection = /*@__PURE__*/ Vec3.create();
28
+ const gjkResult = /*@__PURE__*/ PenetrationDepth.create();
29
+ const ab = /*@__PURE__*/ Vec3.create();
30
+ const transformedShapeA = /*@__PURE__*/ TransformedConvexShape.create();
31
+ const transformedShapeB = /*@__PURE__*/ TransformedConvexShape.create();
32
+ const convexObjectA = /*@__PURE__*/ ConvexRadiusObject.create();
33
+ const vectorAB = /*@__PURE__*/ Vec3.create();
34
+
35
+ const inverseIsometryA = /*@__PURE__*/ Isometry.create();
36
+ const isometryBToA = /*@__PURE__*/ Isometry.create();
37
+
38
+ const pointAB = /*@__PURE__*/ Vec3.create();
39
+ const penetrationAxisWorld = /*@__PURE__*/ Vec3.create();
40
+
41
+ // the results are in world space
42
+ export function collideConvexVsConvex(
43
+ penetrationDepthModule: PenetrationDepthModule,
44
+ collector: CollisionCollector,
45
+ shapeA: ConvexWithcomputeSupportShape,
46
+ shapeB: ConvexWithcomputeSupportShape,
47
+ isometryA: Isometry,
48
+ isometryB: Isometry,
49
+ settings: CollisionSettings,
50
+ bodyA: Body | null,
51
+ bodyB: Body | null,
52
+ initialDirection?: Vec3,
53
+ subShapeIdA: number = 0,
54
+ subShapeIdB: number = 0
55
+ ): void {
56
+ result.reset();
57
+ // @ts-ignore
58
+ result.bodyA = bodyA;
59
+ // @ts-ignore
60
+ result.bodyB = bodyB;
61
+ collector.body2 = bodyB;
62
+
63
+ // get support functions that exclude convex radii
64
+ const shapeAExcludingConvexRadius = shapeA.computeSupportShape(SupportMode.ExcludeConvexRadius, 1);
65
+ const shapeBExcludingConvexRadius = shapeB.computeSupportShape(SupportMode.ExcludeConvexRadius, 1);
66
+
67
+ // get convex radii, including the max separation distance
68
+ const convexRadiusA = shapeAExcludingConvexRadius.getConvexRadius() + settings.maxSeparation;
69
+ const convexRadiusB = shapeBExcludingConvexRadius.getConvexRadius();
70
+
71
+ // create transformed convex objects
72
+ transformedShapeA.transform.matrix.identity();
73
+ transformedShapeA.object = shapeAExcludingConvexRadius;
74
+
75
+ inverseIsometryA.matrix.invertMatrix(isometryA.matrix);
76
+ isometryBToA.matrix.multiplyMatrices(inverseIsometryA.matrix, isometryB.matrix);
77
+
78
+ isometryA.matrix.getTranslation(positionA);
79
+ isometryBToA.matrix.getTranslation(positionB);
80
+ vectorAB.subtractVectors(positionB, positionA);
81
+ searchDirection.copy(vectorAB).normalize();
82
+ if (searchDirection.isNearZero()) {
83
+ searchDirection.fromArray([0, -1, 0]);
84
+ }
85
+
86
+ // transformedShapeB.transform.matrix.copy(isometryB.matrix);
87
+ transformedShapeB.transform.matrix.copy(isometryBToA.matrix);
88
+ transformedShapeB.object = shapeBExcludingConvexRadius;
89
+
90
+ // collide shapes using GJK
91
+ penetrationDepthModule.getPenetrationDepthStepGjk(
92
+ gjkResult,
93
+ transformedShapeA,
94
+ transformedShapeB,
95
+ convexRadiusA,
96
+ convexRadiusB,
97
+ searchDirection,
98
+ settings.collisionTolerance
99
+ );
100
+
101
+ switch (gjkResult.status) {
102
+ case CollisionStatus.Colliding: {
103
+ const point1 = gjkResult.pointA;
104
+ const point2 = gjkResult.pointB;
105
+ const penetration_axis = gjkResult.penetrationAxis;
106
+
107
+ // check if penetration is bigger than early out fraction
108
+ pointAB.subtractVectors(point2, point1);
109
+ const penetration_depth = pointAB.length() - settings.maxSeparation;
110
+ if (-penetration_depth >= collector.getEarlyOutFraction()) {
111
+ result.hasContact = false;
112
+ collector.addMiss();
113
+ return;
114
+ }
115
+
116
+ // correct pointA for the added separation distance
117
+ const penetration_axis_len = penetration_axis.length();
118
+ if (penetration_axis_len > 0.0) {
119
+ point1.addScaled(penetration_axis, -(settings.maxSeparation / penetration_axis_len));
120
+ }
121
+
122
+ // convert to world space
123
+ point1.transformByMat4(isometryA.matrix);
124
+ point2.transformByMat4(isometryA.matrix);
125
+ isometryA.matrix.multiply3x3(penetrationAxisWorld, penetration_axis);
126
+
127
+ // create collision result
128
+ result.subShapeIdA = subShapeIdA;
129
+ result.subShapeIdB = subShapeIdB;
130
+ result.hasContact = true;
131
+ result.penetration = penetration_depth;
132
+ result.contactPointA.copy(point1);
133
+ result.contactPointB.copy(point2);
134
+ result.normalA.copy(penetrationAxisWorld);
135
+
136
+ // TODO: gather faces
137
+
138
+ // notify the collector
139
+ collector.addHit(result);
140
+ return;
141
+ }
142
+
143
+ case CollisionStatus.NotColliding: {
144
+ // definitive no penetration
145
+ result.hasContact = false;
146
+ collector.addMiss();
147
+ return;
148
+ }
149
+
150
+ case CollisionStatus.Indeterminate: {
151
+ // possible deep penetration, use EPA to check
152
+
153
+ // get support functions that include convex radii
154
+ const objectAIncludingConvexRadius = shapeA.computeSupportShape(SupportMode.IncludeConvexRadius, 1);
155
+ const objectBIncludingConvexRadius = shapeB.computeSupportShape(SupportMode.IncludeConvexRadius, 1);
156
+
157
+ // get transformed convex objects
158
+ transformedShapeA.object = objectAIncludingConvexRadius;
159
+ transformedShapeB.object = objectBIncludingConvexRadius;
160
+
161
+ // add max separation distance to convex object A
162
+ convexObjectA.setData(settings.maxSeparation, transformedShapeA);
163
+
164
+ const hitFound = penetrationDepthModule.getPenetrationDepthStepEPA(
165
+ convexObjectA,
166
+ transformedShapeB,
167
+ settings.penetrationTolerance,
168
+ penetrationAxis,
169
+ pointA,
170
+ pointB
171
+ );
172
+
173
+ if (!hitFound) {
174
+ // definitive no penetration
175
+ result.hasContact = false;
176
+ collector.addMiss();
177
+ return;
178
+ }
179
+
180
+ // check if penetration is bigger than early out fraction
181
+ pointAB.subtractVectors(pointB, pointA);
182
+ const penetration_depth = pointAB.length() - settings.maxSeparation;
183
+ if (-penetration_depth >= collector.getEarlyOutFraction()) {
184
+ result.hasContact = false;
185
+ collector.addMiss();
186
+ return;
187
+ }
188
+
189
+ // correct pointA for the added separation distance
190
+ const penetrationAxisLength = penetrationAxis.length();
191
+ if (penetrationAxisLength > 0.0) {
192
+ pointA.addScaled(penetrationAxis, -(settings.maxSeparation / penetrationAxisLength));
193
+ }
194
+
195
+ // convert to world space
196
+ pointA.transformByMat4(isometryA.matrix);
197
+ pointB.transformByMat4(isometryA.matrix);
198
+ isometryA.matrix.multiply3x3(penetrationAxisWorld, penetrationAxis);
199
+
200
+ // create collision result
201
+ result.subShapeIdA = subShapeIdA;
202
+ result.subShapeIdB = subShapeIdB;
203
+ result.hasContact = true;
204
+ result.penetration = penetration_depth;
205
+ result.contactPointA.copy(pointA);
206
+ result.contactPointB.copy(pointB);
207
+ result.normalA.copy(penetrationAxisWorld);
208
+
209
+ // TODO: gather faces
210
+
211
+ // notify the collector
212
+ collector.addHit(result);
213
+
214
+ return;
215
+ }
216
+
217
+ default: {
218
+ throw new Error(`Invalid status: ${gjkResult.status}`);
219
+ }
220
+ }
221
+ }
222
+
223
+ const penetrationAxis = /*@__PURE__*/ Vec3.create();
224
+ const pointA = /*@__PURE__*/ Vec3.create();
225
+ const pointB = /*@__PURE__*/ Vec3.create();
@@ -0,0 +1,236 @@
1
+ import { Isometry } from "../../math/isometry";
2
+ import { Vec3 } from "../../math/vec3";
3
+ import { Face } from "../../physics/manifold/Face";
4
+ import { findContactImp } from "../imp/findContactImp";
5
+ import { ImpResult, ImpStatus } from "../imp/imp";
6
+ import { PenetrationDepth, PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
7
+ import { CollisionCollector, CollisionResult, CollisionSettings, CollisionStatus } from "./collide";
8
+ import {
9
+ ConvexRadiusObject,
10
+ ConvexWithcomputeSupportShape,
11
+ SupportMode,
12
+ TransformedConvexShape,
13
+ } from "../../shape/Convex";
14
+ import { Body } from "../../physics/Body";
15
+
16
+ const result = CollisionResult.create({
17
+ faceA: {
18
+ numVertices: 0,
19
+ buffer: { pool: new Vec3.Pool(32), maxLength: 32 },
20
+ },
21
+ faceB: {
22
+ numVertices: 0,
23
+ buffer: { pool: new Vec3.Pool(32), maxLength: 32 },
24
+ },
25
+ });
26
+
27
+ const positionA = /*@__PURE__*/ Vec3.create();
28
+ const positionB = /*@__PURE__*/ Vec3.create();
29
+ const searchDirection = /*@__PURE__*/ Vec3.create();
30
+ const gjkResult = /*@__PURE__*/ PenetrationDepth.create();
31
+ const impResult = /*@__PURE__*/ ImpResult.create();
32
+ const ab = /*@__PURE__*/ Vec3.create();
33
+ const transformedShapeA = /*@__PURE__*/ TransformedConvexShape.create();
34
+ const transformedShapeB = /*@__PURE__*/ TransformedConvexShape.create();
35
+ const convexObjectA = /*@__PURE__*/ ConvexRadiusObject.create();
36
+ const vectorAB = /*@__PURE__*/ Vec3.create();
37
+
38
+ const inverseIsometryA = /*@__PURE__*/ Isometry.create();
39
+ const isometryBToA = /*@__PURE__*/ Isometry.create();
40
+
41
+ // the results are in world space
42
+ export function collideConvexVsConvexIMP(
43
+ penetrationDepthModule: PenetrationDepthModule,
44
+ collector: CollisionCollector,
45
+ shapeA: ConvexWithcomputeSupportShape,
46
+ shapeB: ConvexWithcomputeSupportShape,
47
+ isometryA: Isometry,
48
+ isometryB: Isometry,
49
+ settings: CollisionSettings,
50
+ bodyA: Body | null,
51
+ bodyB: Body | null,
52
+ initialDirection?: Vec3,
53
+ subShapeIdA: number = 0,
54
+ subShapeIdB: number = 0
55
+ ): void {
56
+ result.reset();
57
+ // @ts-ignore
58
+ result.bodyA = bodyA;
59
+ // @ts-ignore
60
+ result.bodyB = bodyB;
61
+ collector.body2 = bodyB;
62
+
63
+ // get support functions that exclude convex radii
64
+ const shapeAExcludingConvexRadius = shapeA.computeSupportShape(SupportMode.ExcludeConvexRadius, 1);
65
+ const shapeBExcludingConvexRadius = shapeB.computeSupportShape(SupportMode.ExcludeConvexRadius, 1);
66
+
67
+ // get convex radii, including the max separation distance
68
+ const convexRadiusA = shapeAExcludingConvexRadius.getConvexRadius() + settings.maxSeparation;
69
+ const convexRadiusB = shapeBExcludingConvexRadius.getConvexRadius();
70
+
71
+ // create transformed convex objects
72
+ transformedShapeA.transform.matrix.identity();
73
+ transformedShapeA.object = shapeAExcludingConvexRadius;
74
+
75
+ inverseIsometryA.matrix.invertMatrix(isometryA.matrix); // world > A
76
+ isometryBToA.matrix.multiplyMatrices(inverseIsometryA.matrix, isometryB.matrix); // B > A
77
+
78
+ isometryA.matrix.getTranslation(positionA);
79
+ isometryBToA.matrix.getTranslation(positionB);
80
+ vectorAB.subtractVectors(positionB, positionA); // B > A
81
+ searchDirection.copy(vectorAB).normalize();
82
+ if (searchDirection.isNearZero()) {
83
+ searchDirection.fromArray([0, -1, 0]); // A > B
84
+ }
85
+
86
+ // transformedShapeB.transform.matrix.copy(isometryB.matrix);
87
+ transformedShapeB.transform.matrix.copy(isometryBToA.matrix);
88
+ transformedShapeB.object = shapeBExcludingConvexRadius;
89
+
90
+ // collide shapes using GJK
91
+ penetrationDepthModule.getPenetrationDepthStepGjk(
92
+ gjkResult,
93
+ transformedShapeA,
94
+ transformedShapeB,
95
+ convexRadiusA,
96
+ convexRadiusB,
97
+ searchDirection,
98
+ settings.collisionTolerance
99
+ );
100
+
101
+ switch (gjkResult.status) {
102
+ case CollisionStatus.Colliding: {
103
+ // check if penetration is bigger than early out fraction
104
+ pointAB.subtractVectors(gjkResult.pointB, gjkResult.pointA);
105
+ const penetrationDepth = pointAB.length() - settings.maxSeparation;
106
+ if (-penetrationDepth >= collector.getEarlyOutFraction()) {
107
+ result.hasContact = false;
108
+ collector.addMiss();
109
+ return;
110
+ }
111
+
112
+ // correct pointA for the added separation distance
113
+ const penetrationAxisLength = gjkResult.penetrationAxis.length();
114
+ if (penetrationAxisLength > 0.0) {
115
+ gjkResult.pointA.addScaled(gjkResult.penetrationAxis, -(settings.maxSeparation / penetrationAxisLength));
116
+ }
117
+
118
+ // convert to world space
119
+ gjkResult.pointA.transformByMat4(isometryA.matrix);
120
+ gjkResult.pointB.transformByMat4(isometryA.matrix);
121
+ isometryA.matrix.multiply3x3(penetrationAxisWorld, gjkResult.penetrationAxis);
122
+
123
+ // create collision result
124
+ result.subShapeIdA = subShapeIdA;
125
+ result.subShapeIdB = subShapeIdB;
126
+ result.hasContact = true;
127
+ result.penetration = penetrationDepth;
128
+ result.contactPointA.copy(gjkResult.pointA);
129
+ result.contactPointB.copy(gjkResult.pointB);
130
+ result.normalA.copy(penetrationAxisWorld);
131
+
132
+ // TODO: gather faces
133
+
134
+ // notify the collector
135
+ collector.addHit(result);
136
+ return;
137
+ }
138
+
139
+ case CollisionStatus.NotColliding: {
140
+ // definitive no penetration
141
+ result.hasContact = false;
142
+ collector.addMiss();
143
+ return;
144
+ }
145
+
146
+ case CollisionStatus.Indeterminate: {
147
+ // possible deep penetration, use IMP to check
148
+
149
+ // get support functions that include convex radii
150
+ const objectAIncludingConvexRadius = shapeA.computeSupportShape(SupportMode.IncludeConvexRadius, 1);
151
+ const objectBIncludingConvexRadius = shapeB.computeSupportShape(SupportMode.IncludeConvexRadius, 1);
152
+
153
+ // get transformed convex objects
154
+ transformedShapeA.object = objectAIncludingConvexRadius;
155
+ transformedShapeB.object = objectBIncludingConvexRadius;
156
+
157
+ // add max separation distance to convex object A
158
+ convexObjectA.setData(settings.maxSeparation, transformedShapeA);
159
+
160
+ findContactImp(
161
+ impResult,
162
+ convexObjectA,
163
+ transformedShapeB,
164
+ searchDirection,
165
+ 100,
166
+ settings.collisionTolerance,
167
+ false
168
+ );
169
+
170
+ switch (impResult.status) {
171
+ // TODO: what's the difference between these three?
172
+ case ImpStatus.DirectedPenetrationFailed:
173
+ case ImpStatus.Failed:
174
+ case ImpStatus.FailedNoIntersect: {
175
+ // definitive no penetration
176
+ result.hasContact = false;
177
+ collector.addMiss();
178
+ return;
179
+ }
180
+
181
+ // TODO: should iteration limit be considered a failure?
182
+ case ImpStatus.IterationLimit:
183
+ case ImpStatus.OK: {
184
+ // check if penetration is bigger than early out fraction
185
+ pointAB.subtractVectors(impResult.worldContactPointB, impResult.worldContactPointA);
186
+ const penetrationDepth = pointAB.length() - settings.maxSeparation;
187
+ if (-penetrationDepth >= collector.getEarlyOutFraction()) {
188
+ result.hasContact = false;
189
+ collector.addMiss();
190
+ return;
191
+ }
192
+
193
+ // correct pointA for the added separation distance
194
+ const penetrationAxisLength = impResult.penetration_direction.length();
195
+ if (penetrationAxisLength > 0.0) {
196
+ impResult.worldContactPointA.addScaled(
197
+ impResult.penetration_direction,
198
+ -(settings.maxSeparation / penetrationAxisLength)
199
+ );
200
+ }
201
+
202
+ // convert to world space
203
+ impResult.worldContactPointA.transformByMat4(isometryA.matrix);
204
+ impResult.worldContactPointB.transformByMat4(isometryA.matrix);
205
+ isometryA.matrix.multiply3x3(penetrationAxisWorld, impResult.penetration_direction);
206
+
207
+ // create collision result
208
+ result.subShapeIdA = subShapeIdA;
209
+ result.subShapeIdB = subShapeIdB;
210
+ result.hasContact = true;
211
+ result.penetration = penetrationDepth;
212
+ result.contactPointA.copy(impResult.worldContactPointA);
213
+ result.contactPointB.copy(impResult.worldContactPointB);
214
+ result.normalA.copy(penetrationAxisWorld);
215
+
216
+ // TODO: gather faces
217
+
218
+ // notify the collector
219
+ collector.addHit(result);
220
+ return;
221
+ }
222
+
223
+ default: {
224
+ throw new Error(`Invalid status: ${impResult.status}`);
225
+ }
226
+ }
227
+ }
228
+
229
+ default: {
230
+ throw new Error(`Invalid status: ${gjkResult.status}`);
231
+ }
232
+ }
233
+ }
234
+
235
+ const pointAB = /*@__PURE__*/ Vec3.create();
236
+ const penetrationAxisWorld = /*@__PURE__*/ Vec3.create();