reze-engine 0.31.2 → 0.32.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/dist/engine.d.ts +11 -1
- package/dist/engine.d.ts.map +1 -1
- package/dist/engine.js +31 -2
- package/dist/physics/body.d.ts +3 -0
- package/dist/physics/body.d.ts.map +1 -1
- package/dist/physics/body.js +3 -0
- package/dist/physics/constraint.d.ts +0 -32
- package/dist/physics/constraint.d.ts.map +1 -1
- package/dist/physics/constraint.js +0 -32
- package/dist/physics/contact.d.ts.map +1 -1
- package/dist/physics/contact.js +84 -0
- package/dist/physics/physics.d.ts +1 -0
- package/dist/physics/physics.d.ts.map +1 -1
- package/dist/physics/physics.js +35 -3
- package/dist/physics/physics.worker.d.ts +2 -0
- package/dist/physics/physics.worker.d.ts.map +1 -0
- package/dist/physics/physics.worker.js +52 -0
- package/dist/physics/solver.d.ts +9 -1
- package/dist/physics/solver.d.ts.map +1 -1
- package/dist/physics/solver.js +248 -185
- package/dist/physics/worker-physics.d.ts +28 -0
- package/dist/physics/worker-physics.d.ts.map +1 -0
- package/dist/physics/worker-physics.js +91 -0
- package/dist/physics/world.d.ts +2 -1
- package/dist/physics/world.d.ts.map +1 -1
- package/dist/physics/world.js +2 -2
- package/package.json +1 -1
- package/src/engine.ts +33 -3
- package/src/physics/body.ts +4 -0
- package/src/physics/constraint.ts +2 -65
- package/src/physics/contact.ts +80 -0
- package/src/physics/physics.ts +36 -3
- package/src/physics/physics.worker.ts +66 -0
- package/src/physics/solver.ts +979 -905
- package/src/physics/worker-physics.ts +126 -0
- package/src/physics/world.ts +9 -3
package/dist/physics/solver.js
CHANGED
|
@@ -59,7 +59,7 @@ const _bodyMatB = new Float32Array(16);
|
|
|
59
59
|
const _angDiffScratch = new Float32Array(3);
|
|
60
60
|
const _quatScratchA = new Float32Array(4);
|
|
61
61
|
const _quatScratchB = new Float32Array(4);
|
|
62
|
-
export function solveConstraints(store, constraints, contacts, dt, iterations) {
|
|
62
|
+
export function solveConstraints(store, constraints, cache, contacts, dt, iterations) {
|
|
63
63
|
if (dt <= 0)
|
|
64
64
|
return;
|
|
65
65
|
if (constraints.length === 0 && contacts.count === 0)
|
|
@@ -72,23 +72,79 @@ export function solveConstraints(store, constraints, contacts, dt, iterations) {
|
|
|
72
72
|
store.updateInvInertiaWorld();
|
|
73
73
|
const W = store.invInertiaWorld;
|
|
74
74
|
for (let c = 0; c < constraints.length; c++) {
|
|
75
|
-
setupConstraint(constraints[c], store, dt, invDt);
|
|
75
|
+
setupConstraint(constraints[c], c, cache, store, dt, invDt);
|
|
76
76
|
}
|
|
77
77
|
for (let ci = 0; ci < contacts.count; ci++) {
|
|
78
78
|
setupContactRow(contacts.get(ci), lv, av, invMass, W);
|
|
79
79
|
}
|
|
80
80
|
for (let iter = 0; iter < iterations; iter++) {
|
|
81
81
|
for (let c = 0; c < constraints.length; c++) {
|
|
82
|
-
iterateConstraint(
|
|
82
|
+
iterateConstraint(c, cache, lv, av, invMass);
|
|
83
83
|
}
|
|
84
84
|
for (let ci = 0; ci < contacts.count; ci++) {
|
|
85
85
|
iterateContactRow(contacts.get(ci), lv, av, invMass);
|
|
86
86
|
}
|
|
87
87
|
}
|
|
88
88
|
}
|
|
89
|
+
// ── Flat solver cache (SoA) ──────────────────────────────────────────────────
|
|
90
|
+
// One typed-array block instead of a dozen small Float32Arrays per constraint:
|
|
91
|
+
// the 10-iteration hot loop walks memory linearly off a single base pointer
|
|
92
|
+
// instead of pointer-chasing ~1000 scattered objects. Layout below mirrors the
|
|
93
|
+
// old cache fields one-to-one; the math is untouched and bit-identical.
|
|
94
|
+
const F_STRIDE = 108;
|
|
95
|
+
const LEVER_A = 0; // 3
|
|
96
|
+
const LEVER_B = 3; // 3
|
|
97
|
+
const LIN_AXES = 6; // 9
|
|
98
|
+
const LIN_CA = 15; // 9
|
|
99
|
+
const LIN_CB = 24; // 9
|
|
100
|
+
const LIN_JAC = 33; // 3
|
|
101
|
+
const LIN_TGT = 36; // 3
|
|
102
|
+
const LIN_LIMIT_IMP = 39; // 3
|
|
103
|
+
const LIN_SPR_TGT = 42; // 3
|
|
104
|
+
const LIN_SPR_MAX = 45; // 3
|
|
105
|
+
const LIN_SPR_IMP = 48; // 3
|
|
106
|
+
const ANG_AXES = 51; // 9
|
|
107
|
+
const ANG_TGT = 60; // 3
|
|
108
|
+
const ANG_JAC = 63; // 3
|
|
109
|
+
const ANG_SPR_MAX = 66; // 3
|
|
110
|
+
const ANG_SPR_IMP = 69; // 3
|
|
111
|
+
const ANG_WA = 72; // 9
|
|
112
|
+
const ANG_WB = 81; // 9
|
|
113
|
+
const ANG_LIM_AXIS = 90; // 3
|
|
114
|
+
const ANG_LIM_WA = 93; // 3
|
|
115
|
+
const ANG_LIM_WB = 96; // 3
|
|
116
|
+
const ANG_PA_TGT = 99; // 3
|
|
117
|
+
const ANG_PA_IMP = 102; // 3
|
|
118
|
+
const I_STRIDE = 16;
|
|
119
|
+
const I_BODY_A = 0;
|
|
120
|
+
const I_BODY_B = 1;
|
|
121
|
+
const I_SKIP = 2;
|
|
122
|
+
const I_LIN_ACT = 3; // 3
|
|
123
|
+
const I_LIN_SPR_ACT = 6; // 3
|
|
124
|
+
const I_ANG_ACT = 9; // 3
|
|
125
|
+
const I_ANG_PA_ACT = 12; // 3
|
|
126
|
+
const I_ANG_LIM_ACT = 15;
|
|
127
|
+
export class SolverCache {
|
|
128
|
+
constructor(constraints) {
|
|
129
|
+
const n = constraints.length;
|
|
130
|
+
this.F = new Float32Array(n * F_STRIDE);
|
|
131
|
+
this.I = new Int32Array(n * I_STRIDE);
|
|
132
|
+
this.D = new Float64Array(n * 3);
|
|
133
|
+
for (let i = 0; i < n; i++) {
|
|
134
|
+
this.I[i * I_STRIDE + I_BODY_A] = constraints[i].bodyA;
|
|
135
|
+
this.I[i * I_STRIDE + I_BODY_B] = constraints[i].bodyB;
|
|
136
|
+
}
|
|
137
|
+
}
|
|
138
|
+
}
|
|
89
139
|
// SETUP: compute everything that doesn't depend on velocities. Caller
|
|
90
140
|
// guarantees pos/ori don't change between this and the iter loop.
|
|
91
|
-
function setupConstraint(con, store, dt, invDt) {
|
|
141
|
+
function setupConstraint(con, ci, cache, store, dt, invDt) {
|
|
142
|
+
const F = cache.F;
|
|
143
|
+
const I = cache.I;
|
|
144
|
+
const D = cache.D;
|
|
145
|
+
const base = ci * F_STRIDE;
|
|
146
|
+
const ib = ci * I_STRIDE;
|
|
147
|
+
const db = ci * 3;
|
|
92
148
|
const a = con.bodyA;
|
|
93
149
|
const b = con.bodyB;
|
|
94
150
|
const imA = store.invMass[a];
|
|
@@ -96,8 +152,9 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
96
152
|
const W = store.invInertiaWorld;
|
|
97
153
|
const a9 = a * 9;
|
|
98
154
|
const b9 = b * 9;
|
|
99
|
-
|
|
100
|
-
|
|
155
|
+
const skip = imA === 0 && imB === 0;
|
|
156
|
+
I[ib + I_SKIP] = skip ? 1 : 0;
|
|
157
|
+
if (skip)
|
|
101
158
|
return;
|
|
102
159
|
const erpScale = con.isLoop ? LOOP_ERP_SCALE : 1.0;
|
|
103
160
|
buildBodyMat(store, a, _bodyMatA);
|
|
@@ -121,14 +178,14 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
121
178
|
const rBx = _TB[12] - pos[bi + 0];
|
|
122
179
|
const rBy = _TB[13] - pos[bi + 1];
|
|
123
180
|
const rBz = _TB[14] - pos[bi + 2];
|
|
124
|
-
const lA =
|
|
125
|
-
const lB =
|
|
126
|
-
lA
|
|
127
|
-
lA
|
|
128
|
-
lA
|
|
129
|
-
lB
|
|
130
|
-
lB
|
|
131
|
-
lB
|
|
181
|
+
const lA = base + LEVER_A;
|
|
182
|
+
const lB = base + LEVER_B;
|
|
183
|
+
F[lA + 0] = rAx;
|
|
184
|
+
F[lA + 1] = rAy;
|
|
185
|
+
F[lA + 2] = rAz;
|
|
186
|
+
F[lB + 0] = rBx;
|
|
187
|
+
F[lB + 1] = rBy;
|
|
188
|
+
F[lB + 2] = rBz;
|
|
132
189
|
// linearDiff = TA.basis^T · (TB.origin − TA.origin); axes = TA columns 0/1/2.
|
|
133
190
|
const dxw = _TB[12] - _TA[12];
|
|
134
191
|
const dyw = _TB[13] - _TA[13];
|
|
@@ -136,20 +193,20 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
136
193
|
const linDiff0 = _TA[0] * dxw + _TA[1] * dyw + _TA[2] * dzw;
|
|
137
194
|
const linDiff1 = _TA[4] * dxw + _TA[5] * dyw + _TA[6] * dzw;
|
|
138
195
|
const linDiff2 = _TA[8] * dxw + _TA[9] * dyw + _TA[10] * dzw;
|
|
139
|
-
const axes =
|
|
140
|
-
const cA =
|
|
141
|
-
const cB =
|
|
142
|
-
const jac =
|
|
143
|
-
const tgt =
|
|
144
|
-
const act =
|
|
196
|
+
const axes = base + LIN_AXES;
|
|
197
|
+
const cA = base + LIN_CA;
|
|
198
|
+
const cB = base + LIN_CB;
|
|
199
|
+
const jac = base + LIN_JAC;
|
|
200
|
+
const tgt = base + LIN_TGT;
|
|
201
|
+
const act = ib + I_LIN_ACT;
|
|
145
202
|
for (let i = 0; i < 3; i++) {
|
|
146
203
|
const o = i * 3;
|
|
147
204
|
const axx = i === 0 ? _TA[0] : i === 1 ? _TA[4] : _TA[8];
|
|
148
205
|
const axy = i === 0 ? _TA[1] : i === 1 ? _TA[5] : _TA[9];
|
|
149
206
|
const axz = i === 0 ? _TA[2] : i === 1 ? _TA[6] : _TA[10];
|
|
150
|
-
axes
|
|
151
|
-
axes
|
|
152
|
-
axes
|
|
207
|
+
F[axes + o + 0] = axx;
|
|
208
|
+
F[axes + o + 1] = axy;
|
|
209
|
+
F[axes + o + 2] = axz;
|
|
153
210
|
const cAx = rAy * axz - rAz * axy;
|
|
154
211
|
const cAy = rAz * axx - rAx * axz;
|
|
155
212
|
const cAz = rAx * axy - rAy * axx;
|
|
@@ -164,16 +221,16 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
164
221
|
const wBx = W[b9 + 0] * cBx + W[b9 + 1] * cBy + W[b9 + 2] * cBz;
|
|
165
222
|
const wBy = W[b9 + 3] * cBx + W[b9 + 4] * cBy + W[b9 + 5] * cBz;
|
|
166
223
|
const wBz = W[b9 + 6] * cBx + W[b9 + 7] * cBy + W[b9 + 8] * cBz;
|
|
167
|
-
cA
|
|
168
|
-
cA
|
|
169
|
-
cA
|
|
170
|
-
cB
|
|
171
|
-
cB
|
|
172
|
-
cB
|
|
224
|
+
F[cA + o + 0] = wAx;
|
|
225
|
+
F[cA + o + 1] = wAy;
|
|
226
|
+
F[cA + o + 2] = wAz;
|
|
227
|
+
F[cB + o + 0] = wBx;
|
|
228
|
+
F[cB + o + 1] = wBy;
|
|
229
|
+
F[cB + o + 2] = wBz;
|
|
173
230
|
const denom = imA + imB +
|
|
174
231
|
(cAx * wAx + cAy * wAy + cAz * wAz) +
|
|
175
232
|
(cBx * wBx + cBy * wBy + cBz * wBz);
|
|
176
|
-
jac
|
|
233
|
+
F[jac + i] = denom > 0 ? 1 / denom : 0;
|
|
177
234
|
const lo = con.linearMin[i];
|
|
178
235
|
const hi = con.linearMax[i];
|
|
179
236
|
const curr = i === 0 ? linDiff0 : i === 1 ? linDiff1 : linDiff2;
|
|
@@ -199,9 +256,9 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
199
256
|
target = -MAX_LINEAR_CORRECTION_VEL;
|
|
200
257
|
}
|
|
201
258
|
}
|
|
202
|
-
tgt
|
|
203
|
-
act
|
|
204
|
-
|
|
259
|
+
F[tgt + i] = target;
|
|
260
|
+
I[act + i] = denom > 0 ? active : 0;
|
|
261
|
+
F[base + LIN_LIMIT_IMP + i] = 0;
|
|
205
262
|
// A spring on a locked axis is redundant — the bilateral limit row
|
|
206
263
|
// already welds the DOF, and driving it twice overshoots every
|
|
207
264
|
// iteration (PMX rigs routinely put k=100000 springs on locked axes,
|
|
@@ -215,13 +272,13 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
215
272
|
const meff = 1 / denom;
|
|
216
273
|
const c = 2 * SPRING_DAMPING_ZETA * Math.sqrt(k * meff);
|
|
217
274
|
const gamma = c + dt * k;
|
|
218
|
-
|
|
219
|
-
|
|
220
|
-
|
|
221
|
-
|
|
275
|
+
F[base + LIN_SPR_TGT + i] = -(k / gamma) * serr;
|
|
276
|
+
F[base + LIN_SPR_MAX + i] = 1 / (dt * gamma);
|
|
277
|
+
F[base + LIN_SPR_IMP + i] = 0;
|
|
278
|
+
I[ib + I_LIN_SPR_ACT + i] = 1;
|
|
222
279
|
}
|
|
223
280
|
else if (!(lo === hi && con.isLoop)) {
|
|
224
|
-
|
|
281
|
+
I[ib + I_LIN_SPR_ACT + i] = 0;
|
|
225
282
|
}
|
|
226
283
|
}
|
|
227
284
|
// Angular: TA^T · TB → Euler XYZ; axes from TA.col2 × TB.col0.
|
|
@@ -265,65 +322,65 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
265
322
|
zy *= inv;
|
|
266
323
|
zz *= inv;
|
|
267
324
|
}
|
|
268
|
-
const angAxes =
|
|
269
|
-
angAxes
|
|
270
|
-
angAxes
|
|
271
|
-
angAxes
|
|
272
|
-
angAxes
|
|
273
|
-
angAxes
|
|
274
|
-
angAxes
|
|
275
|
-
angAxes
|
|
276
|
-
angAxes
|
|
277
|
-
angAxes
|
|
325
|
+
const angAxes = base + ANG_AXES;
|
|
326
|
+
F[angAxes + 0] = xx;
|
|
327
|
+
F[angAxes + 1] = xy;
|
|
328
|
+
F[angAxes + 2] = xz;
|
|
329
|
+
F[angAxes + 3] = yx;
|
|
330
|
+
F[angAxes + 4] = yy;
|
|
331
|
+
F[angAxes + 5] = yz;
|
|
332
|
+
F[angAxes + 6] = zx;
|
|
333
|
+
F[angAxes + 7] = zy;
|
|
334
|
+
F[angAxes + 8] = zz;
|
|
278
335
|
// Per-axis angular Jacobians with the full tensors: cache I⁻¹·axis per
|
|
279
336
|
// body plus 1/(axᵀ(I⁻¹A+I⁻¹B)ax) per axis.
|
|
280
|
-
const angJac =
|
|
281
|
-
const angWAs =
|
|
282
|
-
const angWBs =
|
|
337
|
+
const angJac = base + ANG_JAC;
|
|
338
|
+
const angWAs = base + ANG_WA;
|
|
339
|
+
const angWBs = base + ANG_WB;
|
|
283
340
|
for (let i = 0; i < 3; i++) {
|
|
284
341
|
const o = i * 3;
|
|
285
|
-
const axx = angAxes
|
|
342
|
+
const axx = F[angAxes + o + 0], axy = F[angAxes + o + 1], axz = F[angAxes + o + 2];
|
|
286
343
|
const wAx = W[a9 + 0] * axx + W[a9 + 1] * axy + W[a9 + 2] * axz;
|
|
287
344
|
const wAy = W[a9 + 3] * axx + W[a9 + 4] * axy + W[a9 + 5] * axz;
|
|
288
345
|
const wAz = W[a9 + 6] * axx + W[a9 + 7] * axy + W[a9 + 8] * axz;
|
|
289
346
|
const wBx = W[b9 + 0] * axx + W[b9 + 1] * axy + W[b9 + 2] * axz;
|
|
290
347
|
const wBy = W[b9 + 3] * axx + W[b9 + 4] * axy + W[b9 + 5] * axz;
|
|
291
348
|
const wBz = W[b9 + 6] * axx + W[b9 + 7] * axy + W[b9 + 8] * axz;
|
|
292
|
-
angWAs
|
|
293
|
-
angWAs
|
|
294
|
-
angWAs
|
|
295
|
-
angWBs
|
|
296
|
-
angWBs
|
|
297
|
-
angWBs
|
|
349
|
+
F[angWAs + o + 0] = wAx;
|
|
350
|
+
F[angWAs + o + 1] = wAy;
|
|
351
|
+
F[angWAs + o + 2] = wAz;
|
|
352
|
+
F[angWBs + o + 0] = wBx;
|
|
353
|
+
F[angWBs + o + 1] = wBy;
|
|
354
|
+
F[angWBs + o + 2] = wBz;
|
|
298
355
|
const denom = axx * (wAx + wBx) + axy * (wAy + wBy) + axz * (wAz + wBz);
|
|
299
|
-
angJac
|
|
356
|
+
F[angJac + i] = denom > 0 ? 1 / denom : 0;
|
|
300
357
|
}
|
|
301
358
|
// Per-axis rows carry only the springs. Sign flip vs linear:
|
|
302
359
|
// d(angDiff)/dt = −(ω_B − ω_A)·ax.
|
|
303
|
-
const angTgt =
|
|
304
|
-
const angAct =
|
|
360
|
+
const angTgt = base + ANG_TGT;
|
|
361
|
+
const angAct = ib + I_ANG_ACT;
|
|
305
362
|
for (let i = 0; i < 3; i++) {
|
|
306
363
|
const idx = i + 3;
|
|
307
364
|
// Springs on locked axes are skipped — the limit row welds those, and
|
|
308
365
|
// double-driving a DOF overshoots every iteration (see the linear loop).
|
|
309
|
-
if (con.springEnabled[idx] && angJac
|
|
310
|
-
// Implicit spring-damper, angular flavor. angJac
|
|
366
|
+
if (con.springEnabled[idx] && F[angJac + i] > 0 && con.angularMin[i] !== con.angularMax[i]) {
|
|
367
|
+
// Implicit spring-damper, angular flavor. F[angJac + i] = 1/denom IS the
|
|
311
368
|
// row's effective inertia. Sign flip vs linear: d(err)/dt = −relAv, so
|
|
312
369
|
// the bias is positive for positive error. cacheAngSpringMaxImp holds
|
|
313
370
|
// the CFM softness s (not a clamp).
|
|
314
371
|
const k = con.springStiffness[idx];
|
|
315
372
|
const serr = _angDiffScratch[i] - con.equilibriumPoint[idx];
|
|
316
|
-
const c = 2 * SPRING_DAMPING_ZETA * Math.sqrt(k * angJac
|
|
373
|
+
const c = 2 * SPRING_DAMPING_ZETA * Math.sqrt(k * F[angJac + i]);
|
|
317
374
|
const gamma = c + dt * k;
|
|
318
|
-
angTgt
|
|
319
|
-
|
|
320
|
-
angAct
|
|
375
|
+
F[angTgt + i] = (k / gamma) * serr;
|
|
376
|
+
F[base + ANG_SPR_MAX + i] = 1 / (dt * gamma);
|
|
377
|
+
I[angAct + i] = 1;
|
|
321
378
|
}
|
|
322
379
|
else {
|
|
323
|
-
angTgt
|
|
324
|
-
angAct
|
|
380
|
+
F[angTgt + i] = 0;
|
|
381
|
+
I[angAct + i] = 0;
|
|
325
382
|
}
|
|
326
|
-
|
|
383
|
+
F[base + ANG_SPR_IMP + i] = 0;
|
|
327
384
|
}
|
|
328
385
|
// Angular limit handling is hybrid. Small violations (the resting-cloth
|
|
329
386
|
// regime) use per-axis euler rows — they converge cleanly and keep resting
|
|
@@ -332,10 +389,10 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
332
389
|
// this port used) become geometrically inconsistent for large errors — near
|
|
333
390
|
// the asin singularity they chase phantom errors and pump angular velocity
|
|
334
391
|
// into the chain instead of converging.
|
|
335
|
-
|
|
336
|
-
|
|
337
|
-
|
|
338
|
-
|
|
392
|
+
I[ib + I_ANG_LIM_ACT] = 0;
|
|
393
|
+
I[ib + I_ANG_PA_ACT + 0] = 0;
|
|
394
|
+
I[ib + I_ANG_PA_ACT + 1] = 0;
|
|
395
|
+
I[ib + I_ANG_PA_ACT + 2] = 0;
|
|
339
396
|
if (imA > 0 || imB > 0) {
|
|
340
397
|
const ex = _angDiffScratch[0], ey = _angDiffScratch[1], ez = _angDiffScratch[2];
|
|
341
398
|
// Free axes (min > max) follow the current angle, i.e. no correction.
|
|
@@ -356,9 +413,9 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
356
413
|
// convergence (more iterations enforce the brake harder).
|
|
357
414
|
for (let i = 0; i < 3; i++) {
|
|
358
415
|
const err = i === 0 ? errX : i === 1 ? errY : errZ;
|
|
359
|
-
|
|
416
|
+
F[base + ANG_PA_IMP + i] = 0;
|
|
360
417
|
if (err === 0) {
|
|
361
|
-
|
|
418
|
+
I[ib + I_ANG_PA_ACT + i] = 0;
|
|
362
419
|
continue;
|
|
363
420
|
}
|
|
364
421
|
let target = err * STOP_ERP * erpScale * invDt;
|
|
@@ -366,8 +423,8 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
366
423
|
target = MAX_ANGULAR_CORRECTION_VEL;
|
|
367
424
|
else if (target < -MAX_ANGULAR_CORRECTION_VEL)
|
|
368
425
|
target = -MAX_ANGULAR_CORRECTION_VEL;
|
|
369
|
-
|
|
370
|
-
|
|
426
|
+
F[base + ANG_PA_TGT + i] = target;
|
|
427
|
+
I[ib + I_ANG_PA_ACT + i] = con.angularMin[i] === con.angularMax[i] ? 1 : 2;
|
|
371
428
|
}
|
|
372
429
|
}
|
|
373
430
|
else if (maxErr > 0) {
|
|
@@ -400,51 +457,57 @@ function setupConstraint(con, store, dt, invDt) {
|
|
|
400
457
|
const invS = 1 / sinHalf;
|
|
401
458
|
const axx = qex * invS, axy = qey * invS, axz = qez * invS;
|
|
402
459
|
// Axis lives in TA's frame; TA's basis columns map it to world.
|
|
403
|
-
const lim =
|
|
404
|
-
lim
|
|
405
|
-
lim
|
|
406
|
-
lim
|
|
407
|
-
const gWA =
|
|
408
|
-
const gWB =
|
|
409
|
-
gWA
|
|
410
|
-
gWA
|
|
411
|
-
gWA
|
|
412
|
-
gWB
|
|
413
|
-
gWB
|
|
414
|
-
gWB
|
|
415
|
-
const gDenom = lim
|
|
416
|
-
|
|
460
|
+
const lim = base + ANG_LIM_AXIS;
|
|
461
|
+
F[lim + 0] = _TA[0] * axx + _TA[4] * axy + _TA[8] * axz;
|
|
462
|
+
F[lim + 1] = _TA[1] * axx + _TA[5] * axy + _TA[9] * axz;
|
|
463
|
+
F[lim + 2] = _TA[2] * axx + _TA[6] * axy + _TA[10] * axz;
|
|
464
|
+
const gWA = base + ANG_LIM_WA;
|
|
465
|
+
const gWB = base + ANG_LIM_WB;
|
|
466
|
+
F[gWA + 0] = W[a9 + 0] * F[lim + 0] + W[a9 + 1] * F[lim + 1] + W[a9 + 2] * F[lim + 2];
|
|
467
|
+
F[gWA + 1] = W[a9 + 3] * F[lim + 0] + W[a9 + 4] * F[lim + 1] + W[a9 + 5] * F[lim + 2];
|
|
468
|
+
F[gWA + 2] = W[a9 + 6] * F[lim + 0] + W[a9 + 7] * F[lim + 1] + W[a9 + 8] * F[lim + 2];
|
|
469
|
+
F[gWB + 0] = W[b9 + 0] * F[lim + 0] + W[b9 + 1] * F[lim + 1] + W[b9 + 2] * F[lim + 2];
|
|
470
|
+
F[gWB + 1] = W[b9 + 3] * F[lim + 0] + W[b9 + 4] * F[lim + 1] + W[b9 + 5] * F[lim + 2];
|
|
471
|
+
F[gWB + 2] = W[b9 + 6] * F[lim + 0] + W[b9 + 7] * F[lim + 1] + W[b9 + 8] * F[lim + 2];
|
|
472
|
+
const gDenom = F[lim + 0] * (F[gWA + 0] + F[gWB + 0]) + F[lim + 1] * (F[gWA + 1] + F[gWB + 1]) + F[lim + 2] * (F[gWA + 2] + F[gWB + 2]);
|
|
473
|
+
D[db + 0] = gDenom > 0 ? 1 / gDenom : 0;
|
|
417
474
|
let target = angle * STOP_ERP * erpScale * invDt;
|
|
418
475
|
if (target > MAX_ANGULAR_CORRECTION_VEL)
|
|
419
476
|
target = MAX_ANGULAR_CORRECTION_VEL;
|
|
420
|
-
|
|
421
|
-
|
|
477
|
+
D[db + 1] = target;
|
|
478
|
+
I[ib + I_ANG_LIM_ACT] = bilateral ? 1 : 2;
|
|
422
479
|
}
|
|
423
480
|
}
|
|
424
481
|
}
|
|
425
|
-
|
|
482
|
+
D[db + 2] = 0;
|
|
426
483
|
}
|
|
427
484
|
// ITER: read cache, compute relVel from current lv/av, apply impulse.
|
|
428
|
-
function iterateConstraint(
|
|
429
|
-
|
|
485
|
+
function iterateConstraint(ci, cache, lv, av, invMass) {
|
|
486
|
+
const F = cache.F;
|
|
487
|
+
const I = cache.I;
|
|
488
|
+
const D = cache.D;
|
|
489
|
+
const base = ci * F_STRIDE;
|
|
490
|
+
const ib = ci * I_STRIDE;
|
|
491
|
+
const db = ci * 3;
|
|
492
|
+
if (I[ib + I_SKIP] === 1)
|
|
430
493
|
return;
|
|
431
|
-
const a =
|
|
432
|
-
const b =
|
|
494
|
+
const a = I[ib + I_BODY_A];
|
|
495
|
+
const b = I[ib + I_BODY_B];
|
|
433
496
|
const ai = a * 3;
|
|
434
497
|
const bi = b * 3;
|
|
435
498
|
const imA = invMass[a];
|
|
436
499
|
const imB = invMass[b];
|
|
437
500
|
// Linear axes — relVel at the offset point: v_pivot = v_CG + ω × r.
|
|
438
|
-
const lA =
|
|
439
|
-
const lB =
|
|
440
|
-
const rAx = lA
|
|
441
|
-
const rBx = lB
|
|
442
|
-
const axes =
|
|
443
|
-
const cA =
|
|
444
|
-
const cB =
|
|
445
|
-
const jac =
|
|
446
|
-
const tgt =
|
|
447
|
-
const act =
|
|
501
|
+
const lA = base + LEVER_A;
|
|
502
|
+
const lB = base + LEVER_B;
|
|
503
|
+
const rAx = F[lA + 0], rAy = F[lA + 1], rAz = F[lA + 2];
|
|
504
|
+
const rBx = F[lB + 0], rBy = F[lB + 1], rBz = F[lB + 2];
|
|
505
|
+
const axes = base + LIN_AXES;
|
|
506
|
+
const cA = base + LIN_CA;
|
|
507
|
+
const cB = base + LIN_CB;
|
|
508
|
+
const jac = base + LIN_JAC;
|
|
509
|
+
const tgt = base + LIN_TGT;
|
|
510
|
+
const act = ib + I_LIN_ACT;
|
|
448
511
|
const vAx = lv[ai + 0] + av[ai + 1] * rAz - av[ai + 2] * rAy;
|
|
449
512
|
const vAy = lv[ai + 1] + av[ai + 2] * rAx - av[ai + 0] * rAz;
|
|
450
513
|
const vAz = lv[ai + 2] + av[ai + 0] * rAy - av[ai + 1] * rAx;
|
|
@@ -454,16 +517,16 @@ function iterateConstraint(con, lv, av, invMass) {
|
|
|
454
517
|
const dvx = vBx - vAx;
|
|
455
518
|
const dvy = vBy - vAy;
|
|
456
519
|
const dvz = vBz - vAz;
|
|
457
|
-
const sprAct =
|
|
458
|
-
const sprTgt =
|
|
459
|
-
const sprMax =
|
|
460
|
-
const sprImp =
|
|
461
|
-
const limImp =
|
|
520
|
+
const sprAct = ib + I_LIN_SPR_ACT;
|
|
521
|
+
const sprTgt = base + LIN_SPR_TGT;
|
|
522
|
+
const sprMax = base + LIN_SPR_MAX;
|
|
523
|
+
const sprImp = base + LIN_SPR_IMP;
|
|
524
|
+
const limImp = base + LIN_LIMIT_IMP;
|
|
462
525
|
for (let i = 0; i < 3; i++) {
|
|
463
|
-
if (!act
|
|
526
|
+
if (!I[act + i] && !I[sprAct + i])
|
|
464
527
|
continue;
|
|
465
528
|
const o = i * 3;
|
|
466
|
-
const axx = axes
|
|
529
|
+
const axx = F[axes + o + 0], axy = F[axes + o + 1], axz = F[axes + o + 2];
|
|
467
530
|
const relVel = dvx * axx + dvy * axy + dvz * axz;
|
|
468
531
|
let j = 0;
|
|
469
532
|
// Limit row. Locked axes (act 1) are bilateral equality joints; ranged
|
|
@@ -471,16 +534,16 @@ function iterateConstraint(con, lv, av, invMass) {
|
|
|
471
534
|
// clamped to the corrective sign, so the stop pushes back into range but
|
|
472
535
|
// never pulls deeper or brakes natural recovery (a bilateral stop acts
|
|
473
536
|
// as a motor and pumps energy into swinging cloth).
|
|
474
|
-
if (act
|
|
475
|
-
const target = tgt
|
|
476
|
-
let dImp = LIMIT_SOFTNESS_LINEAR * (target - relVel) * jac
|
|
477
|
-
if (act
|
|
478
|
-
const old = limImp
|
|
537
|
+
if (I[act + i]) {
|
|
538
|
+
const target = F[tgt + i];
|
|
539
|
+
let dImp = LIMIT_SOFTNESS_LINEAR * (target - relVel) * F[jac + i];
|
|
540
|
+
if (I[act + i] === 2) {
|
|
541
|
+
const old = F[limImp + i];
|
|
479
542
|
let next = old + dImp;
|
|
480
543
|
if (target > 0 ? next < 0 : next > 0)
|
|
481
544
|
next = 0;
|
|
482
545
|
dImp = next - old;
|
|
483
|
-
limImp
|
|
546
|
+
F[limImp + i] = next;
|
|
484
547
|
}
|
|
485
548
|
j += dImp;
|
|
486
549
|
}
|
|
@@ -488,11 +551,11 @@ function iterateConstraint(con, lv, av, invMass) {
|
|
|
488
551
|
// with accumulated λ, dissipative by construction. relVel is refreshed
|
|
489
552
|
// with the limit impulse applied just above (j·denom = j / jac) — driving
|
|
490
553
|
// the spring off the stale value double-corrects the DOF.
|
|
491
|
-
if (sprAct
|
|
492
|
-
const relVelNow = j !== 0 ? relVel + j / jac
|
|
493
|
-
const s = sprMax
|
|
494
|
-
const dImp = (sprTgt
|
|
495
|
-
sprImp
|
|
554
|
+
if (I[sprAct + i]) {
|
|
555
|
+
const relVelNow = j !== 0 ? relVel + j / F[jac + i] : relVel;
|
|
556
|
+
const s = F[sprMax + i]; // CFM softness
|
|
557
|
+
const dImp = (F[sprTgt + i] - relVelNow - s * F[sprImp + i]) / (1 / F[jac + i] + s);
|
|
558
|
+
F[sprImp + i] += dImp;
|
|
496
559
|
j += dImp;
|
|
497
560
|
}
|
|
498
561
|
if (j === 0)
|
|
@@ -501,127 +564,127 @@ function iterateConstraint(con, lv, av, invMass) {
|
|
|
501
564
|
lv[ai + 0] -= j * imA * axx;
|
|
502
565
|
lv[ai + 1] -= j * imA * axy;
|
|
503
566
|
lv[ai + 2] -= j * imA * axz;
|
|
504
|
-
av[ai + 0] -= j * cA
|
|
505
|
-
av[ai + 1] -= j * cA
|
|
506
|
-
av[ai + 2] -= j * cA
|
|
567
|
+
av[ai + 0] -= j * F[cA + o + 0];
|
|
568
|
+
av[ai + 1] -= j * F[cA + o + 1];
|
|
569
|
+
av[ai + 2] -= j * F[cA + o + 2];
|
|
507
570
|
}
|
|
508
571
|
if (imB > 0) {
|
|
509
572
|
lv[bi + 0] += j * imB * axx;
|
|
510
573
|
lv[bi + 1] += j * imB * axy;
|
|
511
574
|
lv[bi + 2] += j * imB * axz;
|
|
512
|
-
av[bi + 0] += j * cB
|
|
513
|
-
av[bi + 1] += j * cB
|
|
514
|
-
av[bi + 2] += j * cB
|
|
575
|
+
av[bi + 0] += j * F[cB + o + 0];
|
|
576
|
+
av[bi + 1] += j * F[cB + o + 1];
|
|
577
|
+
av[bi + 2] += j * F[cB + o + 2];
|
|
515
578
|
}
|
|
516
579
|
}
|
|
517
580
|
// Angular axes — relAv = ω_B − ω_A.
|
|
518
|
-
const angAxes =
|
|
519
|
-
const angJac =
|
|
520
|
-
const angWAs =
|
|
521
|
-
const angWBs =
|
|
522
|
-
const angTgt =
|
|
523
|
-
const angAct =
|
|
581
|
+
const angAxes = base + ANG_AXES;
|
|
582
|
+
const angJac = base + ANG_JAC;
|
|
583
|
+
const angWAs = base + ANG_WA;
|
|
584
|
+
const angWBs = base + ANG_WB;
|
|
585
|
+
const angTgt = base + ANG_TGT;
|
|
586
|
+
const angAct = ib + I_ANG_ACT;
|
|
524
587
|
const dax = av[bi + 0] - av[ai + 0];
|
|
525
588
|
const day = av[bi + 1] - av[ai + 1];
|
|
526
589
|
const daz = av[bi + 2] - av[ai + 2];
|
|
527
|
-
const angSprMax =
|
|
528
|
-
const angSprImp =
|
|
590
|
+
const angSprMax = base + ANG_SPR_MAX;
|
|
591
|
+
const angSprImp = base + ANG_SPR_IMP;
|
|
529
592
|
for (let i = 0; i < 3; i++) {
|
|
530
|
-
if (!angAct
|
|
593
|
+
if (!I[angAct + i])
|
|
531
594
|
continue;
|
|
532
595
|
const o = i * 3;
|
|
533
|
-
const axx = angAxes
|
|
596
|
+
const axx = F[angAxes + o + 0], axy = F[angAxes + o + 1], axz = F[angAxes + o + 2];
|
|
534
597
|
const relAv = dax * axx + day * axy + daz * axz;
|
|
535
598
|
// Implicit spring-damper row (soft constraint, see setup).
|
|
536
|
-
const s = angSprMax
|
|
537
|
-
const j = (angTgt
|
|
538
|
-
angSprImp
|
|
599
|
+
const s = F[angSprMax + i]; // CFM softness
|
|
600
|
+
const j = (F[angTgt + i] - relAv - s * F[angSprImp + i]) / (1 / F[angJac + i] + s);
|
|
601
|
+
F[angSprImp + i] += j;
|
|
539
602
|
if (j === 0)
|
|
540
603
|
continue;
|
|
541
604
|
if (imA > 0) {
|
|
542
|
-
av[ai + 0] -= j * angWAs
|
|
543
|
-
av[ai + 1] -= j * angWAs
|
|
544
|
-
av[ai + 2] -= j * angWAs
|
|
605
|
+
av[ai + 0] -= j * F[angWAs + o + 0];
|
|
606
|
+
av[ai + 1] -= j * F[angWAs + o + 1];
|
|
607
|
+
av[ai + 2] -= j * F[angWAs + o + 2];
|
|
545
608
|
}
|
|
546
609
|
if (imB > 0) {
|
|
547
|
-
av[bi + 0] += j * angWBs
|
|
548
|
-
av[bi + 1] += j * angWBs
|
|
549
|
-
av[bi + 2] += j * angWBs
|
|
610
|
+
av[bi + 0] += j * F[angWBs + o + 0];
|
|
611
|
+
av[bi + 1] += j * F[angWBs + o + 1];
|
|
612
|
+
av[bi + 2] += j * F[angWBs + o + 2];
|
|
550
613
|
}
|
|
551
614
|
}
|
|
552
615
|
// Per-axis angular limit rows (small-violation regime), on the derived
|
|
553
616
|
// euler axes. Sign convention matches the springs: positive target reduces
|
|
554
617
|
// positive error via d(angDiff)/dt = −(ω_B − ω_A)·ax.
|
|
555
|
-
const paAct =
|
|
556
|
-
if (paAct
|
|
557
|
-
const paTgt =
|
|
558
|
-
const paImp =
|
|
618
|
+
const paAct = ib + I_ANG_PA_ACT;
|
|
619
|
+
if (I[paAct + 0] || I[paAct + 1] || I[paAct + 2]) {
|
|
620
|
+
const paTgt = base + ANG_PA_TGT;
|
|
621
|
+
const paImp = base + ANG_PA_IMP;
|
|
559
622
|
for (let i = 0; i < 3; i++) {
|
|
560
|
-
if (!paAct
|
|
623
|
+
if (!I[paAct + i])
|
|
561
624
|
continue;
|
|
562
625
|
const o = i * 3;
|
|
563
|
-
const axx = angAxes
|
|
626
|
+
const axx = F[angAxes + o + 0], axy = F[angAxes + o + 1], axz = F[angAxes + o + 2];
|
|
564
627
|
const relAv = (av[bi + 0] - av[ai + 0]) * axx +
|
|
565
628
|
(av[bi + 1] - av[ai + 1]) * axy +
|
|
566
629
|
(av[bi + 2] - av[ai + 2]) * axz;
|
|
567
|
-
const target = paTgt
|
|
630
|
+
const target = F[paTgt + i];
|
|
568
631
|
// Locked axes (act 1) are welds — full gain, like the 0.16.3 fold;
|
|
569
632
|
// softness only tempers the unilateral stops.
|
|
570
|
-
const soft = paAct
|
|
571
|
-
let j = soft * (target - relAv) * angJac
|
|
572
|
-
if (paAct
|
|
573
|
-
const old = paImp
|
|
633
|
+
const soft = I[paAct + i] === 2 ? LIMIT_SOFTNESS_ANGULAR : 1.0;
|
|
634
|
+
let j = soft * (target - relAv) * F[angJac + i];
|
|
635
|
+
if (I[paAct + i] === 2) {
|
|
636
|
+
const old = F[paImp + i];
|
|
574
637
|
let next = old + j;
|
|
575
638
|
if (target > 0 ? next < 0 : next > 0)
|
|
576
639
|
next = 0;
|
|
577
640
|
j = next - old;
|
|
578
|
-
paImp
|
|
641
|
+
F[paImp + i] = next;
|
|
579
642
|
}
|
|
580
643
|
if (j === 0)
|
|
581
644
|
continue;
|
|
582
645
|
if (imA > 0) {
|
|
583
|
-
av[ai + 0] -= j * angWAs
|
|
584
|
-
av[ai + 1] -= j * angWAs
|
|
585
|
-
av[ai + 2] -= j * angWAs
|
|
646
|
+
av[ai + 0] -= j * F[angWAs + o + 0];
|
|
647
|
+
av[ai + 1] -= j * F[angWAs + o + 1];
|
|
648
|
+
av[ai + 2] -= j * F[angWAs + o + 2];
|
|
586
649
|
}
|
|
587
650
|
if (imB > 0) {
|
|
588
|
-
av[bi + 0] += j * angWBs
|
|
589
|
-
av[bi + 1] += j * angWBs
|
|
590
|
-
av[bi + 2] += j * angWBs
|
|
651
|
+
av[bi + 0] += j * F[angWBs + o + 0];
|
|
652
|
+
av[bi + 1] += j * F[angWBs + o + 1];
|
|
653
|
+
av[bi + 2] += j * F[angWBs + o + 2];
|
|
591
654
|
}
|
|
592
655
|
}
|
|
593
656
|
}
|
|
594
657
|
// Geodesic limit row: drive (ω_B − ω_A)·axis toward the correction target.
|
|
595
658
|
// Unilateral — the accumulated impulse can only push toward the legal
|
|
596
659
|
// region (target is always ≥ 0 along the corrective axis).
|
|
597
|
-
if (
|
|
598
|
-
const lim =
|
|
599
|
-
const nx = lim
|
|
660
|
+
if (I[ib + I_ANG_LIM_ACT] !== 0) {
|
|
661
|
+
const lim = base + ANG_LIM_AXIS;
|
|
662
|
+
const nx = F[lim + 0], ny = F[lim + 1], nz = F[lim + 2];
|
|
600
663
|
// Re-read relAv — the spring rows above may have changed av.
|
|
601
664
|
const relAv = (av[bi + 0] - av[ai + 0]) * nx +
|
|
602
665
|
(av[bi + 1] - av[ai + 1]) * ny +
|
|
603
666
|
(av[bi + 2] - av[ai + 2]) * nz;
|
|
604
|
-
let j = LIMIT_SOFTNESS_ANGULAR * (
|
|
605
|
-
if (
|
|
606
|
-
const old =
|
|
667
|
+
let j = LIMIT_SOFTNESS_ANGULAR * (D[db + 1] - relAv) * D[db + 0];
|
|
668
|
+
if (I[ib + I_ANG_LIM_ACT] === 2) {
|
|
669
|
+
const old = D[db + 2];
|
|
607
670
|
let next = old + j;
|
|
608
671
|
if (next < 0)
|
|
609
672
|
next = 0;
|
|
610
673
|
j = next - old;
|
|
611
|
-
|
|
674
|
+
D[db + 2] = next;
|
|
612
675
|
}
|
|
613
676
|
if (j !== 0) {
|
|
614
|
-
const gWA =
|
|
615
|
-
const gWB =
|
|
677
|
+
const gWA = base + ANG_LIM_WA;
|
|
678
|
+
const gWB = base + ANG_LIM_WB;
|
|
616
679
|
if (imA > 0) {
|
|
617
|
-
av[ai + 0] -= j * gWA
|
|
618
|
-
av[ai + 1] -= j * gWA
|
|
619
|
-
av[ai + 2] -= j * gWA
|
|
680
|
+
av[ai + 0] -= j * F[gWA + 0];
|
|
681
|
+
av[ai + 1] -= j * F[gWA + 1];
|
|
682
|
+
av[ai + 2] -= j * F[gWA + 2];
|
|
620
683
|
}
|
|
621
684
|
if (imB > 0) {
|
|
622
|
-
av[bi + 0] += j * gWB
|
|
623
|
-
av[bi + 1] += j * gWB
|
|
624
|
-
av[bi + 2] += j * gWB
|
|
685
|
+
av[bi + 0] += j * F[gWB + 0];
|
|
686
|
+
av[bi + 1] += j * F[gWB + 1];
|
|
687
|
+
av[bi + 2] += j * F[gWB + 2];
|
|
625
688
|
}
|
|
626
689
|
}
|
|
627
690
|
}
|