@codexo/exojs-physics 0.15.3 → 0.16.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.
- package/README.md +51 -23
- package/dist/esm/Aabb.d.ts +6 -10
- package/dist/esm/Aabb.d.ts.map +1 -0
- package/dist/esm/Aabb.js +9 -5
- package/dist/esm/Aabb.js.map +1 -1
- package/dist/esm/Collider.d.ts +62 -10
- package/dist/esm/Collider.d.ts.map +1 -0
- package/dist/esm/Collider.js +263 -157
- package/dist/esm/Collider.js.map +1 -1
- package/dist/esm/ContactGraph.d.ts +51 -5
- package/dist/esm/ContactGraph.d.ts.map +1 -0
- package/dist/esm/ContactGraph.js +282 -223
- package/dist/esm/ContactGraph.js.map +1 -1
- package/dist/esm/ContactModifier.d.ts +85 -0
- package/dist/esm/ContactModifier.d.ts.map +1 -0
- package/dist/esm/ContactModifier.js +49 -0
- package/dist/esm/ContactModifier.js.map +1 -0
- package/dist/esm/PhysicsBody.d.ts +56 -22
- package/dist/esm/PhysicsBody.d.ts.map +1 -0
- package/dist/esm/PhysicsBody.js +461 -443
- package/dist/esm/PhysicsBody.js.map +1 -1
- package/dist/esm/PhysicsWorld.d.ts +276 -43
- package/dist/esm/PhysicsWorld.d.ts.map +1 -0
- package/dist/esm/PhysicsWorld.js +890 -626
- package/dist/esm/PhysicsWorld.js.map +1 -1
- package/dist/esm/TimeStepper.d.ts +3 -2
- package/dist/esm/TimeStepper.d.ts.map +1 -0
- package/dist/esm/TimeStepper.js +56 -67
- package/dist/esm/TimeStepper.js.map +1 -1
- package/dist/esm/backend/NativePhysicsBackend.d.ts +8 -4
- package/dist/esm/backend/NativePhysicsBackend.d.ts.map +1 -0
- package/dist/esm/backend/NativePhysicsBackend.js +48 -41
- package/dist/esm/backend/NativePhysicsBackend.js.map +1 -1
- package/dist/esm/backend/PhysicsBackend.d.ts +9 -2
- package/dist/esm/backend/PhysicsBackend.d.ts.map +1 -0
- package/dist/esm/binding/BindingRegistry.d.ts +25 -4
- package/dist/esm/binding/BindingRegistry.d.ts.map +1 -0
- package/dist/esm/binding/BindingRegistry.js +70 -36
- package/dist/esm/binding/BindingRegistry.js.map +1 -1
- package/dist/esm/binding/PhysicsBinding.d.ts +22 -5
- package/dist/esm/binding/PhysicsBinding.d.ts.map +1 -0
- package/dist/esm/binding/PhysicsBinding.js +46 -21
- package/dist/esm/binding/PhysicsBinding.js.map +1 -1
- package/dist/esm/broadphase/AabbTreeBroadPhase.d.ts +68 -0
- package/dist/esm/broadphase/AabbTreeBroadPhase.d.ts.map +1 -0
- package/dist/esm/broadphase/AabbTreeBroadPhase.js +149 -0
- package/dist/esm/broadphase/AabbTreeBroadPhase.js.map +1 -0
- package/dist/esm/broadphase/BroadPhase.d.ts +7 -4
- package/dist/esm/broadphase/BroadPhase.d.ts.map +1 -0
- package/dist/esm/broadphase/index.d.ts +2 -1
- package/dist/esm/broadphase/index.d.ts.map +1 -0
- package/dist/esm/collision/CollisionProxy.d.ts +3 -2
- package/dist/esm/collision/CollisionProxy.d.ts.map +1 -0
- package/dist/esm/collision/Manifold.d.ts +5 -4
- package/dist/esm/collision/Manifold.d.ts.map +1 -0
- package/dist/esm/collision/Manifold.js +34 -25
- package/dist/esm/collision/Manifold.js.map +1 -1
- package/dist/esm/collision/chainAdjacency.d.ts +29 -0
- package/dist/esm/collision/chainAdjacency.d.ts.map +1 -0
- package/dist/esm/collision/chainAdjacency.js +44 -0
- package/dist/esm/collision/chainAdjacency.js.map +1 -0
- package/dist/esm/collision/dispatch.d.ts +35 -0
- package/dist/esm/collision/dispatch.d.ts.map +1 -0
- package/dist/esm/collision/dispatch.js +58 -0
- package/dist/esm/collision/dispatch.js.map +1 -0
- package/dist/esm/collision/index.d.ts +2 -0
- package/dist/esm/collision/index.d.ts.map +1 -0
- package/dist/esm/collision/narrowphase.d.ts +5 -2
- package/dist/esm/collision/narrowphase.d.ts.map +1 -0
- package/dist/esm/collision/narrowphase.js +524 -399
- package/dist/esm/collision/narrowphase.js.map +1 -1
- package/dist/esm/collision/segments.d.ts +19 -0
- package/dist/esm/collision/segments.d.ts.map +1 -0
- package/dist/esm/collision/segments.js +36 -0
- package/dist/esm/collision/segments.js.map +1 -0
- package/dist/esm/collision/sweep.d.ts +35 -0
- package/dist/esm/collision/sweep.d.ts.map +1 -0
- package/dist/esm/collision/sweep.js +437 -0
- package/dist/esm/collision/sweep.js.map +1 -0
- package/dist/esm/debug/PhysicsDebugDraw.d.ts +23 -4
- package/dist/esm/debug/PhysicsDebugDraw.d.ts.map +1 -0
- package/dist/esm/debug/PhysicsDebugDraw.js +230 -188
- package/dist/esm/debug/PhysicsDebugDraw.js.map +1 -1
- package/dist/esm/debug/index.d.ts +1 -0
- package/dist/esm/debug/index.d.ts.map +1 -0
- package/dist/esm/debug/index.js +3 -2
- package/dist/esm/events.d.ts +1 -0
- package/dist/esm/events.d.ts.map +1 -0
- package/dist/esm/index.d.ts +1 -0
- package/dist/esm/index.d.ts.map +1 -0
- package/dist/esm/index.js +24 -19
- package/dist/esm/joints/DistanceJoint.d.ts +4 -3
- package/dist/esm/joints/DistanceJoint.d.ts.map +1 -0
- package/dist/esm/joints/DistanceJoint.js +160 -172
- package/dist/esm/joints/DistanceJoint.js.map +1 -1
- package/dist/esm/joints/Joint.d.ts +5 -4
- package/dist/esm/joints/Joint.d.ts.map +1 -0
- package/dist/esm/joints/Joint.js +23 -21
- package/dist/esm/joints/Joint.js.map +1 -1
- package/dist/esm/joints/MouseJoint.d.ts +8 -7
- package/dist/esm/joints/MouseJoint.d.ts.map +1 -0
- package/dist/esm/joints/MouseJoint.js +132 -133
- package/dist/esm/joints/MouseJoint.js.map +1 -1
- package/dist/esm/joints/PrismaticJoint.d.ts +5 -4
- package/dist/esm/joints/PrismaticJoint.d.ts.map +1 -0
- package/dist/esm/joints/PrismaticJoint.js +211 -240
- package/dist/esm/joints/PrismaticJoint.js.map +1 -1
- package/dist/esm/joints/RevoluteJoint.d.ts +4 -3
- package/dist/esm/joints/RevoluteJoint.d.ts.map +1 -0
- package/dist/esm/joints/RevoluteJoint.js +191 -213
- package/dist/esm/joints/RevoluteJoint.js.map +1 -1
- package/dist/esm/joints/WeldJoint.d.ts +3 -2
- package/dist/esm/joints/WeldJoint.d.ts.map +1 -0
- package/dist/esm/joints/WeldJoint.js +156 -152
- package/dist/esm/joints/WeldJoint.js.map +1 -1
- package/dist/esm/joints/WheelJoint.d.ts +4 -3
- package/dist/esm/joints/WheelJoint.d.ts.map +1 -0
- package/dist/esm/joints/WheelJoint.js +228 -255
- package/dist/esm/joints/WheelJoint.js.map +1 -1
- package/dist/esm/math.d.ts +6 -9
- package/dist/esm/math.d.ts.map +1 -0
- package/dist/esm/math.js +37 -39
- package/dist/esm/math.js.map +1 -1
- package/dist/esm/physicsBuildInfo.d.ts +1 -0
- package/dist/esm/physicsBuildInfo.d.ts.map +1 -0
- package/dist/esm/physicsBuildInfo.js +6 -4
- package/dist/esm/physicsBuildInfo.js.map +1 -1
- package/dist/esm/public.d.ts +8 -3
- package/dist/esm/public.d.ts.map +1 -0
- package/dist/esm/query/QueryEngine.d.ts +56 -12
- package/dist/esm/query/QueryEngine.d.ts.map +1 -0
- package/dist/esm/query/QueryEngine.js +494 -237
- package/dist/esm/query/QueryEngine.js.map +1 -1
- package/dist/esm/query/SpatialIndex.d.ts +34 -0
- package/dist/esm/query/SpatialIndex.d.ts.map +1 -0
- package/dist/esm/shapes/AnyShape.d.ts +9 -4
- package/dist/esm/shapes/AnyShape.d.ts.map +1 -0
- package/dist/esm/shapes/BoxShape.d.ts +2 -1
- package/dist/esm/shapes/BoxShape.d.ts.map +1 -0
- package/dist/esm/shapes/BoxShape.js +37 -25
- package/dist/esm/shapes/BoxShape.js.map +1 -1
- package/dist/esm/shapes/CapsuleShape.d.ts +30 -0
- package/dist/esm/shapes/CapsuleShape.d.ts.map +1 -0
- package/dist/esm/shapes/CapsuleShape.js +88 -0
- package/dist/esm/shapes/CapsuleShape.js.map +1 -0
- package/dist/esm/shapes/ChainEdgeShape.d.ts +30 -0
- package/dist/esm/shapes/ChainEdgeShape.d.ts.map +1 -0
- package/dist/esm/shapes/ChainEdgeShape.js +66 -0
- package/dist/esm/shapes/ChainEdgeShape.js.map +1 -0
- package/dist/esm/shapes/ChainShape.d.ts +52 -0
- package/dist/esm/shapes/ChainShape.d.ts.map +1 -0
- package/dist/esm/shapes/ChainShape.js +85 -0
- package/dist/esm/shapes/ChainShape.js.map +1 -0
- package/dist/esm/shapes/CircleShape.d.ts +3 -4
- package/dist/esm/shapes/CircleShape.d.ts.map +1 -0
- package/dist/esm/shapes/CircleShape.js +27 -26
- package/dist/esm/shapes/CircleShape.js.map +1 -1
- package/dist/esm/shapes/PolygonShape.d.ts +7 -8
- package/dist/esm/shapes/PolygonShape.d.ts.map +1 -0
- package/dist/esm/shapes/PolygonShape.js +131 -162
- package/dist/esm/shapes/PolygonShape.js.map +1 -1
- package/dist/esm/shapes/SegmentShape.d.ts +31 -0
- package/dist/esm/shapes/SegmentShape.d.ts.map +1 -0
- package/dist/esm/shapes/SegmentShape.js +62 -0
- package/dist/esm/shapes/SegmentShape.js.map +1 -0
- package/dist/esm/shapes/Shape.d.ts +28 -17
- package/dist/esm/shapes/Shape.d.ts.map +1 -0
- package/dist/esm/shapes/Shape.js +12 -13
- package/dist/esm/shapes/Shape.js.map +1 -1
- package/dist/esm/shapes/convexParts.d.ts +21 -0
- package/dist/esm/shapes/convexParts.d.ts.map +1 -0
- package/dist/esm/shapes/convexParts.js +26 -0
- package/dist/esm/shapes/convexParts.js.map +1 -0
- package/dist/esm/shapes/decompose.d.ts +24 -0
- package/dist/esm/shapes/decompose.d.ts.map +1 -0
- package/dist/esm/shapes/decompose.js +272 -0
- package/dist/esm/shapes/decompose.js.map +1 -0
- package/dist/esm/shapes/index.d.ts +4 -0
- package/dist/esm/shapes/index.d.ts.map +1 -0
- package/dist/esm/solver/ContactSolver.d.ts +12 -5
- package/dist/esm/solver/ContactSolver.d.ts.map +1 -0
- package/dist/esm/solver/ContactSolver.js +425 -471
- package/dist/esm/solver/ContactSolver.js.map +1 -1
- package/dist/esm/solver/tolerances.d.ts +34 -0
- package/dist/esm/solver/tolerances.d.ts.map +1 -0
- package/dist/esm/solver/tolerances.js +38 -0
- package/dist/esm/solver/tolerances.js.map +1 -0
- package/dist/esm/sort.d.ts +5 -4
- package/dist/esm/sort.d.ts.map +1 -0
- package/dist/esm/sort.js +41 -51
- package/dist/esm/sort.js.map +1 -1
- package/dist/esm/types.d.ts +4 -8
- package/dist/esm/types.d.ts.map +1 -0
- package/dist/esm/types.js +15 -17
- package/dist/esm/types.js.map +1 -1
- package/package.json +6 -6
- package/dist/esm/broadphase/SweepAndPrune.d.ts +0 -15
- package/dist/esm/broadphase/SweepAndPrune.js +0 -77
- package/dist/esm/broadphase/SweepAndPrune.js.map +0 -1
- package/dist/esm/debug/index.js.map +0 -1
- package/dist/esm/index.js.map +0 -1
|
@@ -1,490 +1,444 @@
|
|
|
1
|
-
|
|
2
|
-
|
|
3
|
-
|
|
4
|
-
* only produces a manifold while the colliders overlap, so pushing penetration
|
|
5
|
-
* fully to zero lets a resting contact wink out for a frame (free-fall, then
|
|
6
|
-
* re-detect) — a periodic energy spike. Leaving a small slop keeps the contact
|
|
7
|
-
* persistently overlapping and the warm-start cache alive.
|
|
8
|
-
*/
|
|
9
|
-
const slop = 0.25;
|
|
1
|
+
import { contactSlop } from "./tolerances.js";
|
|
2
|
+
|
|
3
|
+
//#region src/solver/ContactSolver.ts
|
|
10
4
|
/**
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
5
|
+
* Cap on the soft-constraint push-out velocity (px/s). Bounds how fast the bias
|
|
6
|
+
* resolves deep penetration so a large overlap cannot fling bodies apart; the
|
|
7
|
+
* Box2D-v3 analogue is `contactSpeed = 3·lengthUnit`, retuned for ExoJS pixels.
|
|
8
|
+
*/
|
|
15
9
|
const maxBiasVelocity = 4;
|
|
16
10
|
/** Reject the 2-point block solve when the contact matrix is worse-conditioned than this (fall back to sequential). */
|
|
17
|
-
const maxConditionNumber =
|
|
11
|
+
const maxConditionNumber = 1e3;
|
|
18
12
|
/**
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
13
|
+
* Per-contact velocity constraint (1-2 points), computed once per **frame** in
|
|
14
|
+
* {@link ContactSolver.prepare} and reused across all sub-steps and passes.
|
|
15
|
+
* Pooled and reused across frames, so a steady-state step constructs no
|
|
16
|
+
* constraint of its own. It is not allocation-free in the wider sense: a double
|
|
17
|
+
* handed to a function the optimiser declines to inline is boxed on the heap for
|
|
18
|
+
* the duration of that call, which is why the hot paths below apply their
|
|
19
|
+
* results in place instead of through helpers.
|
|
20
|
+
*/
|
|
21
|
+
var ContactConstraint = class {
|
|
22
|
+
record;
|
|
23
|
+
bodyA;
|
|
24
|
+
bodyB;
|
|
25
|
+
nx = 0;
|
|
26
|
+
ny = 0;
|
|
27
|
+
tx = 0;
|
|
28
|
+
ty = 0;
|
|
29
|
+
friction = 0;
|
|
30
|
+
restitution = 0;
|
|
31
|
+
pointCount = 0;
|
|
32
|
+
biasRate = 0;
|
|
33
|
+
massScale = 1;
|
|
34
|
+
impulseScale = 0;
|
|
35
|
+
rAx = [0, 0];
|
|
36
|
+
rAy = [0, 0];
|
|
37
|
+
rBx = [0, 0];
|
|
38
|
+
rBy = [0, 0];
|
|
39
|
+
rotAx = [0, 0];
|
|
40
|
+
rotAy = [0, 0];
|
|
41
|
+
rotBx = [0, 0];
|
|
42
|
+
rotBy = [0, 0];
|
|
43
|
+
normalMass = [0, 0];
|
|
44
|
+
tangentMass = [0, 0];
|
|
45
|
+
/** Contact separation at frame start (= −penetration; negative when overlapping). */
|
|
46
|
+
baseSeparation = [0, 0];
|
|
47
|
+
/** Relative normal velocity at frame start (the impact speed the restitution pass keys off). */
|
|
48
|
+
relativeVelocity = [0, 0];
|
|
49
|
+
/** Push-out target normal velocity for the current pass (set per pass from the live separation). */
|
|
50
|
+
velocityBias = [0, 0];
|
|
51
|
+
/**
|
|
52
|
+
* Whether the point's push-out target was clamped to `maxBiasVelocity` this
|
|
53
|
+
* pass. A clamped target is no longer the spring response the soft scaling
|
|
54
|
+
* models, so the single-point solve drops that scaling while it holds.
|
|
55
|
+
*/
|
|
56
|
+
biasClamped = [false, false];
|
|
57
|
+
block = false;
|
|
58
|
+
k11 = 0;
|
|
59
|
+
k12 = 0;
|
|
60
|
+
k22 = 0;
|
|
61
|
+
invK11 = 0;
|
|
62
|
+
invK12 = 0;
|
|
63
|
+
invK22 = 0;
|
|
64
|
+
};
|
|
69
65
|
/**
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
|
|
82
|
-
|
|
83
|
-
|
|
84
|
-
|
|
85
|
-
|
|
86
|
-
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
105
|
-
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
|
|
109
|
-
|
|
110
|
-
|
|
111
|
-
|
|
112
|
-
|
|
113
|
-
|
|
114
|
-
|
|
115
|
-
|
|
116
|
-
|
|
117
|
-
|
|
118
|
-
|
|
119
|
-
|
|
120
|
-
|
|
121
|
-
|
|
122
|
-
|
|
123
|
-
|
|
124
|
-
|
|
125
|
-
|
|
126
|
-
|
|
127
|
-
|
|
128
|
-
|
|
129
|
-
|
|
130
|
-
|
|
131
|
-
|
|
132
|
-
|
|
133
|
-
|
|
134
|
-
|
|
135
|
-
|
|
136
|
-
|
|
137
|
-
|
|
138
|
-
|
|
139
|
-
|
|
140
|
-
|
|
141
|
-
|
|
142
|
-
|
|
143
|
-
|
|
144
|
-
|
|
145
|
-
|
|
146
|
-
|
|
147
|
-
|
|
148
|
-
|
|
149
|
-
|
|
150
|
-
|
|
151
|
-
|
|
152
|
-
|
|
153
|
-
|
|
154
|
-
|
|
155
|
-
|
|
156
|
-
|
|
157
|
-
|
|
158
|
-
|
|
159
|
-
|
|
160
|
-
|
|
161
|
-
|
|
162
|
-
|
|
163
|
-
|
|
164
|
-
|
|
165
|
-
|
|
166
|
-
|
|
167
|
-
|
|
168
|
-
|
|
169
|
-
|
|
170
|
-
|
|
171
|
-
|
|
172
|
-
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
|
|
176
|
-
|
|
177
|
-
|
|
178
|
-
|
|
179
|
-
|
|
180
|
-
|
|
181
|
-
|
|
182
|
-
|
|
183
|
-
|
|
184
|
-
|
|
185
|
-
|
|
186
|
-
|
|
187
|
-
|
|
188
|
-
|
|
189
|
-
|
|
190
|
-
|
|
191
|
-
|
|
192
|
-
|
|
193
|
-
|
|
194
|
-
|
|
195
|
-
|
|
196
|
-
|
|
197
|
-
|
|
198
|
-
|
|
199
|
-
|
|
200
|
-
|
|
201
|
-
|
|
202
|
-
|
|
203
|
-
|
|
204
|
-
|
|
205
|
-
|
|
206
|
-
|
|
207
|
-
|
|
208
|
-
|
|
209
|
-
|
|
210
|
-
|
|
211
|
-
|
|
212
|
-
|
|
213
|
-
|
|
214
|
-
|
|
215
|
-
|
|
216
|
-
|
|
217
|
-
|
|
218
|
-
|
|
219
|
-
|
|
220
|
-
|
|
221
|
-
|
|
222
|
-
|
|
223
|
-
|
|
224
|
-
|
|
225
|
-
|
|
226
|
-
|
|
227
|
-
|
|
228
|
-
|
|
229
|
-
|
|
230
|
-
|
|
231
|
-
|
|
232
|
-
|
|
233
|
-
|
|
234
|
-
|
|
235
|
-
|
|
236
|
-
|
|
237
|
-
|
|
238
|
-
|
|
239
|
-
|
|
240
|
-
|
|
241
|
-
|
|
242
|
-
|
|
243
|
-
|
|
244
|
-
|
|
245
|
-
|
|
246
|
-
|
|
247
|
-
|
|
248
|
-
|
|
249
|
-
|
|
250
|
-
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
254
|
-
|
|
255
|
-
|
|
256
|
-
|
|
257
|
-
|
|
258
|
-
|
|
259
|
-
|
|
260
|
-
|
|
261
|
-
|
|
262
|
-
|
|
263
|
-
|
|
264
|
-
|
|
265
|
-
|
|
266
|
-
|
|
267
|
-
|
|
268
|
-
|
|
269
|
-
|
|
270
|
-
|
|
271
|
-
|
|
272
|
-
|
|
273
|
-
|
|
274
|
-
|
|
275
|
-
|
|
276
|
-
|
|
277
|
-
|
|
278
|
-
|
|
279
|
-
|
|
280
|
-
|
|
281
|
-
|
|
282
|
-
|
|
283
|
-
|
|
284
|
-
|
|
285
|
-
|
|
286
|
-
|
|
287
|
-
|
|
288
|
-
|
|
289
|
-
|
|
290
|
-
|
|
291
|
-
|
|
292
|
-
|
|
293
|
-
|
|
294
|
-
|
|
295
|
-
|
|
296
|
-
|
|
297
|
-
|
|
298
|
-
|
|
299
|
-
|
|
300
|
-
|
|
301
|
-
|
|
302
|
-
|
|
303
|
-
|
|
304
|
-
|
|
305
|
-
|
|
306
|
-
|
|
307
|
-
|
|
308
|
-
|
|
309
|
-
|
|
310
|
-
|
|
311
|
-
|
|
312
|
-
|
|
313
|
-
|
|
314
|
-
|
|
315
|
-
|
|
316
|
-
|
|
317
|
-
|
|
318
|
-
|
|
319
|
-
|
|
320
|
-
|
|
321
|
-
|
|
322
|
-
|
|
323
|
-
|
|
324
|
-
|
|
325
|
-
|
|
326
|
-
|
|
327
|
-
|
|
328
|
-
|
|
329
|
-
|
|
330
|
-
|
|
331
|
-
|
|
332
|
-
|
|
333
|
-
|
|
334
|
-
|
|
335
|
-
|
|
336
|
-
|
|
337
|
-
|
|
338
|
-
|
|
339
|
-
|
|
340
|
-
|
|
341
|
-
|
|
342
|
-
|
|
343
|
-
|
|
344
|
-
|
|
345
|
-
|
|
346
|
-
|
|
347
|
-
|
|
348
|
-
|
|
349
|
-
|
|
350
|
-
|
|
351
|
-
|
|
352
|
-
|
|
353
|
-
|
|
354
|
-
|
|
355
|
-
|
|
356
|
-
|
|
357
|
-
|
|
358
|
-
|
|
359
|
-
|
|
360
|
-
|
|
361
|
-
|
|
362
|
-
|
|
363
|
-
|
|
364
|
-
|
|
365
|
-
|
|
366
|
-
|
|
367
|
-
|
|
368
|
-
|
|
369
|
-
|
|
370
|
-
|
|
371
|
-
|
|
372
|
-
|
|
373
|
-
|
|
374
|
-
|
|
375
|
-
|
|
376
|
-
|
|
377
|
-
|
|
378
|
-
|
|
379
|
-
|
|
380
|
-
|
|
381
|
-
|
|
382
|
-
|
|
383
|
-
|
|
384
|
-
|
|
385
|
-
|
|
386
|
-
|
|
387
|
-
|
|
388
|
-
|
|
389
|
-
|
|
390
|
-
|
|
391
|
-
|
|
392
|
-
|
|
393
|
-
|
|
394
|
-
|
|
395
|
-
|
|
396
|
-
|
|
397
|
-
|
|
398
|
-
|
|
399
|
-
|
|
400
|
-
|
|
401
|
-
|
|
402
|
-
|
|
403
|
-
|
|
404
|
-
|
|
405
|
-
|
|
406
|
-
if (x2 >= 0 && constraint.k12 * x2 + bx >= 0) {
|
|
407
|
-
this._applyBlock(constraint, x1 - a1, x2 - a2);
|
|
408
|
-
constraint.record.normalImpulse[0] = x1;
|
|
409
|
-
constraint.record.normalImpulse[1] = x2;
|
|
410
|
-
return;
|
|
411
|
-
}
|
|
412
|
-
// Case 4 — neither active (separating). Only valid when both residuals are non-negative.
|
|
413
|
-
if (bx >= 0 && by >= 0) {
|
|
414
|
-
this._applyBlock(constraint, -a1, -a2);
|
|
415
|
-
constraint.record.normalImpulse[0] = 0;
|
|
416
|
-
constraint.record.normalImpulse[1] = 0;
|
|
417
|
-
}
|
|
418
|
-
}
|
|
419
|
-
/** Apply the two-point block impulse deltas `(d1, d2)` along the normal at both contact points. */
|
|
420
|
-
_applyBlock(constraint, d1, d2) {
|
|
421
|
-
const bodyA = constraint.bodyA;
|
|
422
|
-
const bodyB = constraint.bodyB;
|
|
423
|
-
const nx = constraint.nx;
|
|
424
|
-
const ny = constraint.ny;
|
|
425
|
-
applyImpulse(bodyA, bodyB, n(constraint.rotAx, 0), n(constraint.rotAy, 0), n(constraint.rotBx, 0), n(constraint.rotBy, 0), d1 * nx, d1 * ny);
|
|
426
|
-
applyImpulse(bodyA, bodyB, n(constraint.rotAx, 1), n(constraint.rotAy, 1), n(constraint.rotBx, 1), n(constraint.rotBy, 1), d2 * nx, d2 * ny);
|
|
427
|
-
}
|
|
428
|
-
_acquire() {
|
|
429
|
-
if (this._count === this._constraints.length) {
|
|
430
|
-
this._constraints.push(new ContactConstraint());
|
|
431
|
-
}
|
|
432
|
-
return this._at(this._count++);
|
|
433
|
-
}
|
|
434
|
-
/**
|
|
435
|
-
* Pooled constraint at `ci`. Every caller indexes within `0..this._count-1`
|
|
436
|
-
* and `_count` never exceeds the pool length, so the entry always exists; the
|
|
437
|
-
* throw is unreachable and only discharges `noUncheckedIndexedAccess` without a
|
|
438
|
-
* cast or non-null assertion.
|
|
439
|
-
*/
|
|
440
|
-
_at(ci) {
|
|
441
|
-
const constraint = this._constraints[ci];
|
|
442
|
-
if (constraint === undefined) {
|
|
443
|
-
throw new RangeError(`ContactSolver: constraint pool index ${ci} out of range.`);
|
|
444
|
-
}
|
|
445
|
-
return constraint;
|
|
446
|
-
}
|
|
447
|
-
}
|
|
66
|
+
* Native warm-started **TGS-Soft** contact solver (Box2D-v3 "soft step"). Driven
|
|
67
|
+
* by {@link PhysicsWorld} as a sub-stepping loop: detection runs once per frame,
|
|
68
|
+
* then {@link prepare} builds the per-contact constraints (effective masses,
|
|
69
|
+
* frame-start separation + impact velocity, soft factors, and a 2×2 block matrix
|
|
70
|
+
* for two-point manifolds) **once**, and each sub-step runs
|
|
71
|
+
* {@link warmStart} → {@link solveVelocities}(useBias=true) → integrate positions
|
|
72
|
+
* → {@link solveVelocities}(useBias=false) relax. A final {@link applyRestitution}
|
|
73
|
+
* pass adds bounce above the threshold.
|
|
74
|
+
*
|
|
75
|
+
* The position constraint is folded into the velocity solve as a **soft bias**
|
|
76
|
+
* (a damped-spring push-out whose stiffness is decoupled from the iteration
|
|
77
|
+
* count), recomputed each pass from the live per-body delta position/rotation -
|
|
78
|
+
* so there is no separate NGS geometric pass. The contact anchors are rotated by
|
|
79
|
+
* the live sub-step rotation each pass, so a tilting stack's restoring torque is
|
|
80
|
+
* computed against its current orientation (without this the tilt grows instead
|
|
81
|
+
* of being corrected). Two-point manifolds still solve the normal impulses **as a
|
|
82
|
+
* 2×2 block LCP** (Box2D-style), which propagates stack loads far better than
|
|
83
|
+
* solving the points sequentially; the block path carries the soft push-out as
|
|
84
|
+
* its velocity target (hard mass scale), while single-point contacts use the full
|
|
85
|
+
* soft mass/impulse scaling for as long as their push-out target stays inside
|
|
86
|
+
* the cap, and the hard scale once it is clamped. Accumulated impulses are
|
|
87
|
+
* written back to each {@link ContactRecord} for warm-starting the next frame.
|
|
88
|
+
*
|
|
89
|
+
* Internal to {@link NativePhysicsBackend}; not part of the public surface.
|
|
90
|
+
*/
|
|
91
|
+
var ContactSolver = class {
|
|
92
|
+
_constraints = [];
|
|
93
|
+
_count = 0;
|
|
94
|
+
/**
|
|
95
|
+
* Build the per-contact constraints for this frame from the touching solid
|
|
96
|
+
* contacts. `h` is the sub-step duration (`fixedDelta / subStepCount`); the
|
|
97
|
+
* soft factors are derived from it together with `contactHertz`/`dampingRatio`.
|
|
98
|
+
* Runs once per frame (detection is not repeated per sub-step).
|
|
99
|
+
*/
|
|
100
|
+
prepare(contacts, h, contactHertz, dampingRatio) {
|
|
101
|
+
this._count = 0;
|
|
102
|
+
const omega = 2 * Math.PI * contactHertz;
|
|
103
|
+
const a1 = 2 * dampingRatio + h * omega;
|
|
104
|
+
const a2 = h * omega * a1;
|
|
105
|
+
const a3 = 1 / (1 + a2);
|
|
106
|
+
const biasRate = contactHertz > 0 ? omega / a1 : 0;
|
|
107
|
+
const massScale = contactHertz > 0 ? a2 * a3 : 1;
|
|
108
|
+
const impulseScale = contactHertz > 0 ? a3 : 0;
|
|
109
|
+
for (const contact of contacts) {
|
|
110
|
+
const manifold = contact.manifold;
|
|
111
|
+
const pointCount = manifold.pointCount;
|
|
112
|
+
if (pointCount === 0 || !contact.enabled) continue;
|
|
113
|
+
const bodyA = contact.a.body;
|
|
114
|
+
const bodyB = contact.b.body;
|
|
115
|
+
if (bodyA.isSleeping || bodyB.isSleeping) continue;
|
|
116
|
+
const constraint = this._acquire();
|
|
117
|
+
const nx = manifold.normalX;
|
|
118
|
+
const ny = manifold.normalY;
|
|
119
|
+
const tx = -ny;
|
|
120
|
+
const ty = nx;
|
|
121
|
+
constraint.record = contact;
|
|
122
|
+
constraint.bodyA = bodyA;
|
|
123
|
+
constraint.bodyB = bodyB;
|
|
124
|
+
constraint.nx = nx;
|
|
125
|
+
constraint.ny = ny;
|
|
126
|
+
constraint.tx = tx;
|
|
127
|
+
constraint.ty = ty;
|
|
128
|
+
constraint.friction = contact.friction;
|
|
129
|
+
constraint.restitution = contact.restitution;
|
|
130
|
+
constraint.pointCount = pointCount;
|
|
131
|
+
constraint.block = false;
|
|
132
|
+
constraint.biasRate = biasRate;
|
|
133
|
+
constraint.massScale = massScale;
|
|
134
|
+
constraint.impulseScale = impulseScale;
|
|
135
|
+
const mA = bodyA.invMass;
|
|
136
|
+
const mB = bodyB.invMass;
|
|
137
|
+
const iA = bodyA.invInertia;
|
|
138
|
+
const iB = bodyB.invInertia;
|
|
139
|
+
const comAx = bodyA.worldCenterOfMassX;
|
|
140
|
+
const comAy = bodyA.worldCenterOfMassY;
|
|
141
|
+
const comBx = bodyB.worldCenterOfMassX;
|
|
142
|
+
const comBy = bodyB.worldCenterOfMassY;
|
|
143
|
+
for (let i = 0; i < pointCount; i++) {
|
|
144
|
+
const point = i === 0 ? manifold.points[0] : manifold.points[1];
|
|
145
|
+
const rAx = point.x - comAx;
|
|
146
|
+
const rAy = point.y - comAy;
|
|
147
|
+
const rBx = point.x - comBx;
|
|
148
|
+
const rBy = point.y - comBy;
|
|
149
|
+
constraint.rAx[i] = rAx;
|
|
150
|
+
constraint.rAy[i] = rAy;
|
|
151
|
+
constraint.rBx[i] = rBx;
|
|
152
|
+
constraint.rBy[i] = rBy;
|
|
153
|
+
constraint.rotAx[i] = rAx;
|
|
154
|
+
constraint.rotAy[i] = rAy;
|
|
155
|
+
constraint.rotBx[i] = rBx;
|
|
156
|
+
constraint.rotBy[i] = rBy;
|
|
157
|
+
const rnA = rAx * ny - rAy * nx;
|
|
158
|
+
const rnB = rBx * ny - rBy * nx;
|
|
159
|
+
const kn = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
|
160
|
+
const rtA = rAx * ty - rAy * tx;
|
|
161
|
+
const rtB = rBx * ty - rBy * tx;
|
|
162
|
+
const kt = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
|
|
163
|
+
constraint.normalMass[i] = kn > 0 ? 1 / kn : 0;
|
|
164
|
+
constraint.tangentMass[i] = kt > 0 ? 1 / kt : 0;
|
|
165
|
+
constraint.baseSeparation[i] = -point.penetration;
|
|
166
|
+
constraint.velocityBias[i] = 0;
|
|
167
|
+
constraint.relativeVelocity[i] = normalVelocity(bodyA, bodyB, rAx, rAy, rBx, rBy, nx, ny);
|
|
168
|
+
}
|
|
169
|
+
if (pointCount === 2) {
|
|
170
|
+
const rn1A = n(constraint.rAx, 0) * ny - n(constraint.rAy, 0) * nx;
|
|
171
|
+
const rn1B = n(constraint.rBx, 0) * ny - n(constraint.rBy, 0) * nx;
|
|
172
|
+
const rn2A = n(constraint.rAx, 1) * ny - n(constraint.rAy, 1) * nx;
|
|
173
|
+
const rn2B = n(constraint.rBx, 1) * ny - n(constraint.rBy, 1) * nx;
|
|
174
|
+
const k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
|
|
175
|
+
const k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
|
|
176
|
+
const k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
|
|
177
|
+
const det = k11 * k22 - k12 * k12;
|
|
178
|
+
if (det > 0 && k11 * k11 < maxConditionNumber * det) {
|
|
179
|
+
const invDet = 1 / det;
|
|
180
|
+
constraint.k11 = k11;
|
|
181
|
+
constraint.k12 = k12;
|
|
182
|
+
constraint.k22 = k22;
|
|
183
|
+
constraint.invK11 = invDet * k22;
|
|
184
|
+
constraint.invK12 = -invDet * k12;
|
|
185
|
+
constraint.invK22 = invDet * k11;
|
|
186
|
+
constraint.block = true;
|
|
187
|
+
}
|
|
188
|
+
}
|
|
189
|
+
}
|
|
190
|
+
}
|
|
191
|
+
/** Re-apply the cached impulses from the previous frame (warm-starting); runs each sub-step. */
|
|
192
|
+
warmStart() {
|
|
193
|
+
for (let ci = 0; ci < this._count; ci++) {
|
|
194
|
+
const constraint = this._at(ci);
|
|
195
|
+
const bodyA = constraint.bodyA;
|
|
196
|
+
const bodyB = constraint.bodyB;
|
|
197
|
+
this._updateArms(constraint);
|
|
198
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
199
|
+
const normalImpulse = n(constraint.record.normalImpulse, i);
|
|
200
|
+
const tangentImpulse = n(constraint.record.tangentImpulse, i);
|
|
201
|
+
const jx = normalImpulse * constraint.nx + tangentImpulse * constraint.tx;
|
|
202
|
+
const jy = normalImpulse * constraint.ny + tangentImpulse * constraint.ty;
|
|
203
|
+
applyImpulse(bodyA, bodyB, n(constraint.rotAx, i), n(constraint.rotAy, i), n(constraint.rotBx, i), n(constraint.rotBy, i), jx, jy);
|
|
204
|
+
}
|
|
205
|
+
}
|
|
206
|
+
}
|
|
207
|
+
/**
|
|
208
|
+
* One velocity pass over every contact: the normal constraint (block when
|
|
209
|
+
* possible) then friction (Coulomb cone). With `useBias` the normal solve
|
|
210
|
+
* carries the soft push-out bias (the main pass, recomputed from the live
|
|
211
|
+
* separation); without it the relax pass drives the normal velocity to zero so
|
|
212
|
+
* the bias velocity does not remain in the bodies as injected energy.
|
|
213
|
+
*/
|
|
214
|
+
solveVelocities(useBias) {
|
|
215
|
+
for (let ci = 0; ci < this._count; ci++) this._solveVelocityContact(this._at(ci), useBias);
|
|
216
|
+
}
|
|
217
|
+
/**
|
|
218
|
+
* Restitution pass, run once after the sub-step loop. For each loaded point
|
|
219
|
+
* whose frame-start impact speed exceeded the threshold, add the impulse that
|
|
220
|
+
* brings the post-solve normal velocity to `−restitution·impactSpeed`. Kept
|
|
221
|
+
* separate from the main solve so resting contacts (impact below threshold) do
|
|
222
|
+
* not micro-bounce.
|
|
223
|
+
*/
|
|
224
|
+
applyRestitution() {
|
|
225
|
+
for (let ci = 0; ci < this._count; ci++) {
|
|
226
|
+
const constraint = this._at(ci);
|
|
227
|
+
if (constraint.restitution === 0) continue;
|
|
228
|
+
const bodyA = constraint.bodyA;
|
|
229
|
+
const bodyB = constraint.bodyB;
|
|
230
|
+
const nx = constraint.nx;
|
|
231
|
+
const ny = constraint.ny;
|
|
232
|
+
this._updateArms(constraint);
|
|
233
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
234
|
+
const relativeVelocity = n(constraint.relativeVelocity, i);
|
|
235
|
+
if (relativeVelocity > -1 || n(constraint.record.normalImpulse, i) <= 0) continue;
|
|
236
|
+
const rAx = n(constraint.rotAx, i);
|
|
237
|
+
const rAy = n(constraint.rotAy, i);
|
|
238
|
+
const rBx = n(constraint.rotBx, i);
|
|
239
|
+
const rBy = n(constraint.rotBy, i);
|
|
240
|
+
const vn = normalVelocity(bodyA, bodyB, rAx, rAy, rBx, rBy, nx, ny);
|
|
241
|
+
const oldNormal = n(constraint.record.normalImpulse, i);
|
|
242
|
+
const newNormal = Math.max(0, oldNormal - n(constraint.normalMass, i) * (vn + constraint.restitution * relativeVelocity));
|
|
243
|
+
const deltaNormal = newNormal - oldNormal;
|
|
244
|
+
constraint.record.normalImpulse[i] = newNormal;
|
|
245
|
+
applyImpulse(bodyA, bodyB, rAx, rAy, rBx, rBy, deltaNormal * nx, deltaNormal * ny);
|
|
246
|
+
}
|
|
247
|
+
}
|
|
248
|
+
}
|
|
249
|
+
/** Refresh the live (rotated) contact arms from each body's accumulated sub-step rotation. */
|
|
250
|
+
_updateArms(constraint) {
|
|
251
|
+
const cosA = constraint.bodyA._deltaCos;
|
|
252
|
+
const sinA = constraint.bodyA._deltaSin;
|
|
253
|
+
const cosB = constraint.bodyB._deltaCos;
|
|
254
|
+
const sinB = constraint.bodyB._deltaSin;
|
|
255
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
256
|
+
const rAx = n(constraint.rAx, i);
|
|
257
|
+
const rAy = n(constraint.rAy, i);
|
|
258
|
+
const rBx = n(constraint.rBx, i);
|
|
259
|
+
const rBy = n(constraint.rBy, i);
|
|
260
|
+
constraint.rotAx[i] = rAx * cosA - rAy * sinA;
|
|
261
|
+
constraint.rotAy[i] = rAx * sinA + rAy * cosA;
|
|
262
|
+
constraint.rotBx[i] = rBx * cosB - rBy * sinB;
|
|
263
|
+
constraint.rotBy[i] = rBx * sinB + rBy * cosB;
|
|
264
|
+
}
|
|
265
|
+
}
|
|
266
|
+
_solveVelocityContact(constraint, useBias) {
|
|
267
|
+
this._updateArms(constraint);
|
|
268
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
269
|
+
const excess = -currentSeparation(constraint, i) - contactSlop;
|
|
270
|
+
const target = useBias && excess > 0 ? constraint.biasRate * excess : 0;
|
|
271
|
+
constraint.velocityBias[i] = Math.min(target, maxBiasVelocity);
|
|
272
|
+
constraint.biasClamped[i] = target > maxBiasVelocity;
|
|
273
|
+
}
|
|
274
|
+
if (constraint.block) this._solveNormalBlock(constraint);
|
|
275
|
+
else this._solveNormalSequential(constraint, useBias);
|
|
276
|
+
this._solveFriction(constraint);
|
|
277
|
+
}
|
|
278
|
+
/** Per-point friction: clamp the accumulated tangent impulse to the Coulomb cone `±μ·normalImpulse`. */
|
|
279
|
+
_solveFriction(constraint) {
|
|
280
|
+
const bodyA = constraint.bodyA;
|
|
281
|
+
const bodyB = constraint.bodyB;
|
|
282
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
283
|
+
const rAx = n(constraint.rotAx, i);
|
|
284
|
+
const rAy = n(constraint.rotAy, i);
|
|
285
|
+
const rBx = n(constraint.rotBx, i);
|
|
286
|
+
const rBy = n(constraint.rotBy, i);
|
|
287
|
+
const vtX = bodyB.linearVelocityX - bodyB.angularVelocity * rBy - (bodyA.linearVelocityX - bodyA.angularVelocity * rAy);
|
|
288
|
+
const vtY = bodyB.linearVelocityY + bodyB.angularVelocity * rBx - (bodyA.linearVelocityY + bodyA.angularVelocity * rAx);
|
|
289
|
+
const vt = vtX * constraint.tx + vtY * constraint.ty;
|
|
290
|
+
const maxFriction = constraint.friction * n(constraint.record.normalImpulse, i);
|
|
291
|
+
const oldTangent = n(constraint.record.tangentImpulse, i);
|
|
292
|
+
const newTangent = clamp(oldTangent - n(constraint.tangentMass, i) * vt, -maxFriction, maxFriction);
|
|
293
|
+
const deltaTangent = newTangent - oldTangent;
|
|
294
|
+
constraint.record.tangentImpulse[i] = newTangent;
|
|
295
|
+
applyImpulse(bodyA, bodyB, rAx, rAy, rBx, rBy, deltaTangent * constraint.tx, deltaTangent * constraint.ty);
|
|
296
|
+
}
|
|
297
|
+
}
|
|
298
|
+
/**
|
|
299
|
+
* Single-point (or ill-conditioned) soft normal solve, accumulated and clamped
|
|
300
|
+
* ≥ 0. The Box2D-v3 incremental form: with the soft bias active the impulse is
|
|
301
|
+
* `−normalMass·massScale·(vn − bias) − impulseScale·accumulated`, which both
|
|
302
|
+
* pushes out the penetration and bleeds a little stored impulse (the damped
|
|
303
|
+
* spring). The relax pass passes `useBias=false`, collapsing it to the hard
|
|
304
|
+
* `−normalMass·vn` (no bias, full mass, no impulse decay).
|
|
305
|
+
*
|
|
306
|
+
* A point whose push-out target was clamped to `maxBiasVelocity` is solved
|
|
307
|
+
* with the hard scale as well: the soft scaling models a spring, and once the
|
|
308
|
+
* target is capped the point would otherwise reach only a fraction of an
|
|
309
|
+
* already limited push-out. That deficit grows with the load, so past the
|
|
310
|
+
* acceleration at which the cap binds the contact would sink without bound
|
|
311
|
+
* instead of settling into a slightly deeper limit cycle.
|
|
312
|
+
*/
|
|
313
|
+
_solveNormalSequential(constraint, useBias) {
|
|
314
|
+
const bodyA = constraint.bodyA;
|
|
315
|
+
const bodyB = constraint.bodyB;
|
|
316
|
+
const nx = constraint.nx;
|
|
317
|
+
const ny = constraint.ny;
|
|
318
|
+
for (let i = 0; i < constraint.pointCount; i++) {
|
|
319
|
+
const rAx = n(constraint.rotAx, i);
|
|
320
|
+
const rAy = n(constraint.rotAy, i);
|
|
321
|
+
const rBx = n(constraint.rotBx, i);
|
|
322
|
+
const rBy = n(constraint.rotBy, i);
|
|
323
|
+
const vn = normalVelocity(bodyA, bodyB, rAx, rAy, rBx, rBy, nx, ny);
|
|
324
|
+
const oldNormal = n(constraint.record.normalImpulse, i);
|
|
325
|
+
const clamped = constraint.biasClamped[i] ?? false;
|
|
326
|
+
const massScale = useBias && !clamped ? constraint.massScale : 1;
|
|
327
|
+
const impulseScale = useBias && !clamped ? constraint.impulseScale : 0;
|
|
328
|
+
const impulse = -n(constraint.normalMass, i) * massScale * (vn - n(constraint.velocityBias, i)) - impulseScale * oldNormal;
|
|
329
|
+
const newNormal = Math.max(0, oldNormal + impulse);
|
|
330
|
+
const deltaNormal = newNormal - oldNormal;
|
|
331
|
+
constraint.record.normalImpulse[i] = newNormal;
|
|
332
|
+
applyImpulse(bodyA, bodyB, rAx, rAy, rBx, rBy, deltaNormal * nx, deltaNormal * ny);
|
|
333
|
+
}
|
|
334
|
+
}
|
|
335
|
+
/**
|
|
336
|
+
* Two-point block normal solve (Box2D LCP): find both new normal impulses
|
|
337
|
+
* `x ≥ 0` so the post-solve normal velocities equal the push-out targets
|
|
338
|
+
* (`velocityBias`, zero in the relax pass) and are complementary, trying the
|
|
339
|
+
* four corners (both active, one active, none). The block path carries the
|
|
340
|
+
* soft push-out as its target but solves with a hard mass scale.
|
|
341
|
+
*/
|
|
342
|
+
_solveNormalBlock(constraint) {
|
|
343
|
+
const bodyA = constraint.bodyA;
|
|
344
|
+
const bodyB = constraint.bodyB;
|
|
345
|
+
const nx = constraint.nx;
|
|
346
|
+
const ny = constraint.ny;
|
|
347
|
+
const a1 = n(constraint.record.normalImpulse, 0);
|
|
348
|
+
const a2 = n(constraint.record.normalImpulse, 1);
|
|
349
|
+
const vn1 = normalVelocity(bodyA, bodyB, n(constraint.rotAx, 0), n(constraint.rotAy, 0), n(constraint.rotBx, 0), n(constraint.rotBy, 0), nx, ny);
|
|
350
|
+
const vn2 = normalVelocity(bodyA, bodyB, n(constraint.rotAx, 1), n(constraint.rotAy, 1), n(constraint.rotBx, 1), n(constraint.rotBy, 1), nx, ny);
|
|
351
|
+
const bx = vn1 - n(constraint.velocityBias, 0) - (constraint.k11 * a1 + constraint.k12 * a2);
|
|
352
|
+
const by = vn2 - n(constraint.velocityBias, 1) - (constraint.k12 * a1 + constraint.k22 * a2);
|
|
353
|
+
let x1 = -(constraint.invK11 * bx + constraint.invK12 * by);
|
|
354
|
+
let x2 = -(constraint.invK12 * bx + constraint.invK22 * by);
|
|
355
|
+
let solved = x1 >= 0 && x2 >= 0;
|
|
356
|
+
if (!solved) {
|
|
357
|
+
x1 = -n(constraint.normalMass, 0) * bx;
|
|
358
|
+
x2 = 0;
|
|
359
|
+
solved = x1 >= 0 && constraint.k12 * x1 + by >= 0;
|
|
360
|
+
}
|
|
361
|
+
if (!solved) {
|
|
362
|
+
x1 = 0;
|
|
363
|
+
x2 = -n(constraint.normalMass, 1) * by;
|
|
364
|
+
solved = x2 >= 0 && constraint.k12 * x2 + bx >= 0;
|
|
365
|
+
}
|
|
366
|
+
if (!solved) {
|
|
367
|
+
if (bx < 0 || by < 0) return;
|
|
368
|
+
x1 = 0;
|
|
369
|
+
x2 = 0;
|
|
370
|
+
}
|
|
371
|
+
const d1 = x1 - a1;
|
|
372
|
+
const d2 = x2 - a2;
|
|
373
|
+
const jx = (d1 + d2) * nx;
|
|
374
|
+
const jy = (d1 + d2) * ny;
|
|
375
|
+
const torqueA = (n(constraint.rotAx, 0) * ny - n(constraint.rotAy, 0) * nx) * d1 + (n(constraint.rotAx, 1) * ny - n(constraint.rotAy, 1) * nx) * d2;
|
|
376
|
+
const torqueB = (n(constraint.rotBx, 0) * ny - n(constraint.rotBy, 0) * nx) * d1 + (n(constraint.rotBx, 1) * ny - n(constraint.rotBy, 1) * nx) * d2;
|
|
377
|
+
bodyA.linearVelocityX -= jx * bodyA.invMass;
|
|
378
|
+
bodyA.linearVelocityY -= jy * bodyA.invMass;
|
|
379
|
+
bodyA.angularVelocity -= torqueA * bodyA.invInertia;
|
|
380
|
+
bodyB.linearVelocityX += jx * bodyB.invMass;
|
|
381
|
+
bodyB.linearVelocityY += jy * bodyB.invMass;
|
|
382
|
+
bodyB.angularVelocity += torqueB * bodyB.invInertia;
|
|
383
|
+
constraint.record.normalImpulse[0] = x1;
|
|
384
|
+
constraint.record.normalImpulse[1] = x2;
|
|
385
|
+
}
|
|
386
|
+
_acquire() {
|
|
387
|
+
if (this._count === this._constraints.length) this._constraints.push(new ContactConstraint());
|
|
388
|
+
return this._at(this._count++);
|
|
389
|
+
}
|
|
390
|
+
/**
|
|
391
|
+
* Pooled constraint at `ci`. Every caller indexes within `0..this._count-1`
|
|
392
|
+
* and `_count` never exceeds the pool length, so the entry always exists; the
|
|
393
|
+
* throw is unreachable and only discharges `noUncheckedIndexedAccess` without a
|
|
394
|
+
* cast or non-null assertion.
|
|
395
|
+
*/
|
|
396
|
+
_at(ci) {
|
|
397
|
+
const constraint = this._constraints[ci];
|
|
398
|
+
if (constraint === void 0) throw new RangeError(`ContactSolver: constraint pool index ${ci} out of range.`);
|
|
399
|
+
return constraint;
|
|
400
|
+
}
|
|
401
|
+
};
|
|
448
402
|
/**
|
|
449
|
-
|
|
450
|
-
|
|
451
|
-
|
|
452
|
-
|
|
453
|
-
|
|
454
|
-
|
|
403
|
+
* Live contact separation at point `i`, recomputed from each body's accumulated
|
|
404
|
+
* delta position plus the rotation-induced shift of the (rotated) anchor since
|
|
405
|
+
* frame start, projected onto the normal. Negative when overlapping. This is what
|
|
406
|
+
* folds the position constraint into the velocity solve without a separate
|
|
407
|
+
* geometric pass or re-running detection per sub-step.
|
|
408
|
+
*/
|
|
455
409
|
const currentSeparation = (constraint, i) => {
|
|
456
|
-
|
|
457
|
-
|
|
458
|
-
|
|
459
|
-
|
|
460
|
-
|
|
461
|
-
|
|
462
|
-
|
|
463
|
-
return n(constraint.baseSeparation, i) + (dispBx - dispAx) * constraint.nx + (dispBy - dispAy) * constraint.ny;
|
|
410
|
+
const bodyA = constraint.bodyA;
|
|
411
|
+
const bodyB = constraint.bodyB;
|
|
412
|
+
const dispAx = bodyA._deltaPosX + n(constraint.rotAx, i) - n(constraint.rAx, i);
|
|
413
|
+
const dispAy = bodyA._deltaPosY + n(constraint.rotAy, i) - n(constraint.rAy, i);
|
|
414
|
+
const dispBx = bodyB._deltaPosX + n(constraint.rotBx, i) - n(constraint.rBx, i);
|
|
415
|
+
const dispBy = bodyB._deltaPosY + n(constraint.rotBy, i) - n(constraint.rBy, i);
|
|
416
|
+
return n(constraint.baseSeparation, i) + (dispBx - dispAx) * constraint.nx + (dispBy - dispAy) * constraint.ny;
|
|
464
417
|
};
|
|
465
418
|
/** Relative normal velocity at a contact point: `dot(vB + ωB×rB − vA − ωA×rA, n)`. */
|
|
466
419
|
const normalVelocity = (bodyA, bodyB, rAx, rAy, rBx, rBy, nx, ny) => {
|
|
467
|
-
|
|
468
|
-
|
|
469
|
-
|
|
420
|
+
const dvx = bodyB.linearVelocityX - bodyB.angularVelocity * rBy - (bodyA.linearVelocityX - bodyA.angularVelocity * rAy);
|
|
421
|
+
const dvy = bodyB.linearVelocityY + bodyB.angularVelocity * rBx - (bodyA.linearVelocityY + bodyA.angularVelocity * rAx);
|
|
422
|
+
return dvx * nx + dvy * ny;
|
|
470
423
|
};
|
|
471
424
|
/** Apply impulse `(jx, jy)` to B and its negation to A about their contact arms. */
|
|
472
425
|
const applyImpulse = (bodyA, bodyB, rAx, rAy, rBx, rBy, jx, jy) => {
|
|
473
|
-
|
|
474
|
-
|
|
475
|
-
|
|
476
|
-
|
|
477
|
-
|
|
478
|
-
|
|
426
|
+
bodyA.linearVelocityX -= jx * bodyA.invMass;
|
|
427
|
+
bodyA.linearVelocityY -= jy * bodyA.invMass;
|
|
428
|
+
bodyA.angularVelocity -= (rAx * jy - rAy * jx) * bodyA.invInertia;
|
|
429
|
+
bodyB.linearVelocityX += jx * bodyB.invMass;
|
|
430
|
+
bodyB.linearVelocityY += jy * bodyB.invMass;
|
|
431
|
+
bodyB.angularVelocity += (rBx * jy - rBy * jx) * bodyB.invInertia;
|
|
479
432
|
};
|
|
480
433
|
const clamp = (value, low, high) => Math.min(Math.max(value, low), high);
|
|
481
434
|
/**
|
|
482
|
-
|
|
483
|
-
|
|
484
|
-
|
|
485
|
-
|
|
486
|
-
|
|
435
|
+
* In-bounds read of a fixed-size (length-2) per-point scratch array. Every
|
|
436
|
+
* caller indexes within `0..pointCount-1` (pointCount ≤ 2), so the element
|
|
437
|
+
* always exists; the `0` fallback only discharges `noUncheckedIndexedAccess`
|
|
438
|
+
* for the unreachable case and keeps the `??` out of caller complexity.
|
|
439
|
+
*/
|
|
487
440
|
const n = (arr, i) => arr[i] ?? 0;
|
|
488
441
|
|
|
442
|
+
//#endregion
|
|
489
443
|
export { ContactSolver };
|
|
490
|
-
//# sourceMappingURL=ContactSolver.js.map
|
|
444
|
+
//# sourceMappingURL=ContactSolver.js.map
|