@woosh/meep-engine 2.154.0 → 2.155.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/package.json +1 -1
- package/src/core/geom/vec3/v3_array_copy.d.ts +3 -3
- package/src/core/geom/vec3/v3_array_copy.d.ts.map +1 -1
- package/src/core/geom/vec3/v3_array_copy.js +2 -2
- package/src/core/geom/vec3/v3_cross.d.ts +17 -0
- package/src/core/geom/vec3/v3_cross.d.ts.map +1 -0
- package/src/core/geom/vec3/v3_cross.js +20 -0
- package/src/core/geom/vec3/v3_subtract.d.ts +16 -0
- package/src/core/geom/vec3/v3_subtract.d.ts.map +1 -0
- package/src/core/geom/vec3/v3_subtract.js +19 -0
- package/src/engine/graphics/ecs/decal/v2/FPDecalSystem.d.ts.map +1 -1
- package/src/engine/graphics/ecs/decal/v2/FPDecalSystem.js +8 -0
- package/src/engine/physics/gjk/gjk_epa_penetration.d.ts +12 -8
- package/src/engine/physics/gjk/gjk_epa_penetration.d.ts.map +1 -1
- package/src/engine/physics/gjk/gjk_epa_penetration.js +451 -158
package/package.json
CHANGED
|
@@ -6,7 +6,7 @@
|
|
|
6
6
|
"description": "Pure JavaScript game engine. Fully featured and production ready.",
|
|
7
7
|
"type": "module",
|
|
8
8
|
"author": "Alexander Goldring",
|
|
9
|
-
"version": "2.
|
|
9
|
+
"version": "2.155.0",
|
|
10
10
|
"main": "build/meep.module.js",
|
|
11
11
|
"module": "build/meep.module.js",
|
|
12
12
|
"exports": {
|
|
@@ -1,9 +1,9 @@
|
|
|
1
1
|
/**
|
|
2
2
|
*
|
|
3
|
-
* @param {number[]|Float32Array|Uint32Array|Int32Array} target
|
|
3
|
+
* @param {number[]|Float32Array|Float64Array|Uint32Array|Int32Array} target
|
|
4
4
|
* @param {number} target_offset
|
|
5
|
-
* @param {number[]|Float32Array|Uint32Array|Int32Array} source
|
|
5
|
+
* @param {number[]|Float32Array|Float64Array|Uint32Array|Int32Array} source
|
|
6
6
|
* @param {number} source_offset
|
|
7
7
|
*/
|
|
8
|
-
export function v3_array_copy(target: number[] | Float32Array | Uint32Array | Int32Array, target_offset: number, source: number[] | Float32Array | Uint32Array | Int32Array, source_offset: number): void;
|
|
8
|
+
export function v3_array_copy(target: number[] | Float32Array | Float64Array | Uint32Array | Int32Array, target_offset: number, source: number[] | Float32Array | Float64Array | Uint32Array | Int32Array, source_offset: number): void;
|
|
9
9
|
//# sourceMappingURL=v3_array_copy.d.ts.map
|
|
@@ -1 +1 @@
|
|
|
1
|
-
{"version":3,"file":"v3_array_copy.d.ts","sourceRoot":"","sources":["../../../../../src/core/geom/vec3/v3_array_copy.js"],"names":[],"mappings":"AAAA;;;;;;GAMG;AACH,sCALW,MAAM,EAAE,GAAC,YAAY,GAAC,WAAW,GAAC,UAAU,
|
|
1
|
+
{"version":3,"file":"v3_array_copy.d.ts","sourceRoot":"","sources":["../../../../../src/core/geom/vec3/v3_array_copy.js"],"names":[],"mappings":"AAAA;;;;;;GAMG;AACH,sCALW,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,GAAC,WAAW,GAAC,UAAU,iBACzD,MAAM,UACN,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,GAAC,WAAW,GAAC,UAAU,iBACzD,MAAM,QAWhB"}
|
|
@@ -1,8 +1,8 @@
|
|
|
1
1
|
/**
|
|
2
2
|
*
|
|
3
|
-
* @param {number[]|Float32Array|Uint32Array|Int32Array} target
|
|
3
|
+
* @param {number[]|Float32Array|Float64Array|Uint32Array|Int32Array} target
|
|
4
4
|
* @param {number} target_offset
|
|
5
|
-
* @param {number[]|Float32Array|Uint32Array|Int32Array} source
|
|
5
|
+
* @param {number[]|Float32Array|Float64Array|Uint32Array|Int32Array} source
|
|
6
6
|
* @param {number} source_offset
|
|
7
7
|
*/
|
|
8
8
|
export function v3_array_copy(target, target_offset, source, source_offset) {
|
|
@@ -0,0 +1,17 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Cross product `a × b`, written to `result[result_offset .. result_offset+2]`.
|
|
3
|
+
*
|
|
4
|
+
* Scalar-argument companion to {@link v3_cross_array} (which takes both operands
|
|
5
|
+
* at array offsets): use this when the operands are already in registers/locals,
|
|
6
|
+
* to avoid staging them into a buffer just to cross them. Allocation-free.
|
|
7
|
+
*
|
|
8
|
+
* @param {number[]|Float32Array|Float64Array} result destination
|
|
9
|
+
* @param {number} result_offset offset into `result`
|
|
10
|
+
* @param {number} ax @param {number} ay @param {number} az first vector
|
|
11
|
+
* @param {number} bx @param {number} by @param {number} bz second vector
|
|
12
|
+
*
|
|
13
|
+
* @author Alex Goldring
|
|
14
|
+
* @copyright Company Named Limited (c) 2026
|
|
15
|
+
*/
|
|
16
|
+
export function v3_cross(result: number[] | Float32Array | Float64Array, result_offset: number, ax: number, ay: number, az: number, bx: number, by: number, bz: number): void;
|
|
17
|
+
//# sourceMappingURL=v3_cross.d.ts.map
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
{"version":3,"file":"v3_cross.d.ts","sourceRoot":"","sources":["../../../../../src/core/geom/vec3/v3_cross.js"],"names":[],"mappings":"AAAA;;;;;;;;;;;;;;GAcG;AACH,iCARW,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,iBAClC,MAAM,MACN,MAAM,MAAa,MAAM,MAAa,MAAM,MAC5C,MAAM,MAAa,MAAM,MAAa,MAAM,QAStD"}
|
|
@@ -0,0 +1,20 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Cross product `a × b`, written to `result[result_offset .. result_offset+2]`.
|
|
3
|
+
*
|
|
4
|
+
* Scalar-argument companion to {@link v3_cross_array} (which takes both operands
|
|
5
|
+
* at array offsets): use this when the operands are already in registers/locals,
|
|
6
|
+
* to avoid staging them into a buffer just to cross them. Allocation-free.
|
|
7
|
+
*
|
|
8
|
+
* @param {number[]|Float32Array|Float64Array} result destination
|
|
9
|
+
* @param {number} result_offset offset into `result`
|
|
10
|
+
* @param {number} ax @param {number} ay @param {number} az first vector
|
|
11
|
+
* @param {number} bx @param {number} by @param {number} bz second vector
|
|
12
|
+
*
|
|
13
|
+
* @author Alex Goldring
|
|
14
|
+
* @copyright Company Named Limited (c) 2026
|
|
15
|
+
*/
|
|
16
|
+
export function v3_cross(result, result_offset, ax, ay, az, bx, by, bz) {
|
|
17
|
+
result[result_offset] = ay * bz - az * by;
|
|
18
|
+
result[result_offset + 1] = az * bx - ax * bz;
|
|
19
|
+
result[result_offset + 2] = ax * by - ay * bx;
|
|
20
|
+
}
|
|
@@ -0,0 +1,16 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Vector difference `a − b`, written to `result[result_offset .. result_offset+2]`.
|
|
3
|
+
*
|
|
4
|
+
* Scalar-argument, out-parameter form: lets callers compute an edge/offset vector
|
|
5
|
+
* straight into a scratch buffer without allocating a temporary. Allocation-free.
|
|
6
|
+
*
|
|
7
|
+
* @param {number[]|Float32Array|Float64Array} result destination
|
|
8
|
+
* @param {number} result_offset offset into `result`
|
|
9
|
+
* @param {number} ax @param {number} ay @param {number} az minuend
|
|
10
|
+
* @param {number} bx @param {number} by @param {number} bz subtrahend
|
|
11
|
+
*
|
|
12
|
+
* @author Alex Goldring
|
|
13
|
+
* @copyright Company Named Limited (c) 2026
|
|
14
|
+
*/
|
|
15
|
+
export function v3_subtract(result: number[] | Float32Array | Float64Array, result_offset: number, ax: number, ay: number, az: number, bx: number, by: number, bz: number): void;
|
|
16
|
+
//# sourceMappingURL=v3_subtract.d.ts.map
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
{"version":3,"file":"v3_subtract.d.ts","sourceRoot":"","sources":["../../../../../src/core/geom/vec3/v3_subtract.js"],"names":[],"mappings":"AAAA;;;;;;;;;;;;;GAaG;AACH,oCARW,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,iBAClC,MAAM,MACN,MAAM,MAAa,MAAM,MAAa,MAAM,MAC5C,MAAM,MAAa,MAAM,MAAa,MAAM,QAStD"}
|
|
@@ -0,0 +1,19 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Vector difference `a − b`, written to `result[result_offset .. result_offset+2]`.
|
|
3
|
+
*
|
|
4
|
+
* Scalar-argument, out-parameter form: lets callers compute an edge/offset vector
|
|
5
|
+
* straight into a scratch buffer without allocating a temporary. Allocation-free.
|
|
6
|
+
*
|
|
7
|
+
* @param {number[]|Float32Array|Float64Array} result destination
|
|
8
|
+
* @param {number} result_offset offset into `result`
|
|
9
|
+
* @param {number} ax @param {number} ay @param {number} az minuend
|
|
10
|
+
* @param {number} bx @param {number} by @param {number} bz subtrahend
|
|
11
|
+
*
|
|
12
|
+
* @author Alex Goldring
|
|
13
|
+
* @copyright Company Named Limited (c) 2026
|
|
14
|
+
*/
|
|
15
|
+
export function v3_subtract(result, result_offset, ax, ay, az, bx, by, bz) {
|
|
16
|
+
result[result_offset] = ax - bx;
|
|
17
|
+
result[result_offset + 1] = ay - by;
|
|
18
|
+
result[result_offset + 2] = az - bz;
|
|
19
|
+
}
|
|
@@ -1 +1 @@
|
|
|
1
|
-
{"version":3,"file":"FPDecalSystem.d.ts","sourceRoot":"","sources":["../../../../../../../src/engine/graphics/ecs/decal/v2/FPDecalSystem.js"],"names":[],"mappings":"
|
|
1
|
+
{"version":3,"file":"FPDecalSystem.d.ts","sourceRoot":"","sources":["../../../../../../../src/engine/graphics/ecs/decal/v2/FPDecalSystem.js"],"names":[],"mappings":"AAqOA;;GAEG;AACH;IAGI;;;OAGG;IACH,4BA0CC;IAhDD,SAAgB;IASZ,kDAAsC;IAEtC;;;;OAIG;IACH,wBAUE;IAEF;;;;OAIG;IACH,iBAAmC;IAEnC;;;;OAIG;IACH,iBAAsB;IAEtB;;;;OAIG;IACH,oBAAiC;IAGrC,gCASC;IAED,iCAEC;IAED;;;;;;OAMG;IACH,4BALW,MAAM,EAAE,iBACR,MAAM,UACN,MAAM,EAAE,GACN,MAAM,CA2DlB;IAED;;;;;;;;;;;OAWG;IACH,kBAVW,MAAM,YACN,MAAM,YACN,MAAM,eACN,MAAM,eACN,MAAM,eACN,MAAM,uEAGJ;QAAC,MAAM,EAAC,MAAM,CAAC;QAAC,SAAS,EAAC,KAAK,CAAC;QAAC,OAAO,EAAE,aAAa,CAAA;KAAC,EAAE,CAkGtE;CACJ;sCAjcqC,iDAAiD;oBAtBnE,sCAAsC;0BAwBhC,wCAAwC;sBAM5C,YAAY;2BAtBP,4CAA4C;8BASzC,8CAA8C"}
|
|
@@ -21,6 +21,7 @@ import { SurfacePoint3 } from "../../../../../core/geom/3d/SurfacePoint3.js";
|
|
|
21
21
|
import { computeStringHash } from "../../../../../core/primitives/strings/computeStringHash.js";
|
|
22
22
|
import { AssetManager } from "../../../../asset/AssetManager.js";
|
|
23
23
|
import { GameAssetType } from "../../../../asset/GameAssetType.js";
|
|
24
|
+
import { ImageRGBADataLoader } from "../../../../asset/loaders/image/ImageRGBADataLoader.js";
|
|
24
25
|
import { AbstractContextSystem } from "../../../../ecs/system/AbstractContextSystem.js";
|
|
25
26
|
import { SystemEntityContext } from "../../../../ecs/system/SystemEntityContext.js";
|
|
26
27
|
import { Transform } from "../../../../ecs/transform/Transform.js";
|
|
@@ -282,6 +283,13 @@ export class FPDecalSystem extends AbstractContextSystem {
|
|
|
282
283
|
|
|
283
284
|
async startup(em) {
|
|
284
285
|
this.__fp_plugin = await this.__engine.plugins.acquire(ForwardPlusRenderingPlugin);
|
|
286
|
+
|
|
287
|
+
const am = this.__engine.assetManager;
|
|
288
|
+
if (!am.hasLoaderForType(GameAssetType.Image)) {
|
|
289
|
+
await am.registerLoader(GameAssetType.Image, new ImageRGBADataLoader());
|
|
290
|
+
}
|
|
291
|
+
|
|
292
|
+
|
|
285
293
|
}
|
|
286
294
|
|
|
287
295
|
async shutdown(em) {
|
|
@@ -3,14 +3,18 @@
|
|
|
3
3
|
* Writes the unit separation normal (B→A) into `out` and returns the depth
|
|
4
4
|
* (positive on overlap, 0 if separated — `out` untouched on 0).
|
|
5
5
|
*
|
|
6
|
-
* @param {Float64Array|number[]} out length ≥ 3
|
|
7
|
-
* @param {
|
|
8
|
-
* @param {
|
|
6
|
+
* @param {Float64Array|number[]} out destination for the unit normal, length ≥ 3
|
|
7
|
+
* @param {SupportProvider} A world-space support provider (e.g. PosedShape)
|
|
8
|
+
* @param {SupportProvider} B world-space support provider (e.g. PosedShape)
|
|
9
9
|
* @returns {number} penetration depth, or 0 if separated
|
|
10
10
|
*/
|
|
11
|
-
export function gjk_epa_penetration(out: Float64Array | number[], A:
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
})
|
|
11
|
+
export function gjk_epa_penetration(out: Float64Array | number[], A: SupportProvider, B: SupportProvider): number;
|
|
12
|
+
/**
|
|
13
|
+
* A convex shape exposing a world-space support mapping: `support(out, off, dx,
|
|
14
|
+
* dy, dz)` writes the farthest point of the shape in direction `(dx, dy, dz)`
|
|
15
|
+
* into `out[off..off+2]` (e.g. {@link PosedShape }).
|
|
16
|
+
*/
|
|
17
|
+
export type SupportProvider = {
|
|
18
|
+
support: (arg0: Float64Array, arg1: number, arg2: number, arg3: number, arg4: number) => void;
|
|
19
|
+
};
|
|
16
20
|
//# sourceMappingURL=gjk_epa_penetration.d.ts.map
|
|
@@ -1 +1 @@
|
|
|
1
|
-
{"version":3,"file":"gjk_epa_penetration.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/gjk/gjk_epa_penetration.js"],"names":[],"mappings":"
|
|
1
|
+
{"version":3,"file":"gjk_epa_penetration.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/gjk/gjk_epa_penetration.js"],"names":[],"mappings":"AAmhBA;;;;;;;;;GASG;AACH,yCALW,YAAY,GAAC,MAAM,EAAE,KACrB,eAAe,KACf,eAAe,GACb,MAAM,CAQlB;;;;;;8BAndY;IAAE,OAAO,SAAW,YAAY,QAAE,MAAM,QAAE,MAAM,QAAE,MAAM,QAAE,MAAM,KAAG,IAAI,CAAA;CAAE"}
|
|
@@ -1,3 +1,10 @@
|
|
|
1
|
+
import { v3_array_copy } from "../../../core/geom/vec3/v3_array_copy.js";
|
|
2
|
+
import { v3_cross } from "../../../core/geom/vec3/v3_cross.js";
|
|
3
|
+
import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
|
|
4
|
+
import { v3_length_sqr } from "../../../core/geom/vec3/v3_length_sqr.js";
|
|
5
|
+
import { v3_subtract } from "../../../core/geom/vec3/v3_subtract.js";
|
|
6
|
+
import { v3_triple_cross_product } from "../../../core/geom/vec3/v3_triple_cross_product.js";
|
|
7
|
+
|
|
1
8
|
/**
|
|
2
9
|
* Robust GJK + EPA penetration for convex shapes — the narrowphase's
|
|
3
10
|
* convex-fallback and concave-per-triangle penetration query. Replaced the
|
|
@@ -13,11 +20,13 @@
|
|
|
13
20
|
* difference's closest face to the origin. No cross-frame state → reset-and-
|
|
14
21
|
* resimulate determinism preserved.
|
|
15
22
|
*
|
|
16
|
-
*
|
|
17
|
-
*
|
|
18
|
-
*
|
|
19
|
-
*
|
|
20
|
-
*
|
|
23
|
+
* Allocation-free hot path: the GJK simplex lives in a flat `Float64Array(12)`
|
|
24
|
+
* and the EPA polytope in pooled flat typed arrays (vertices, face vertex-indices,
|
|
25
|
+
* face normals, face distances, horizon edges). All vector math goes through
|
|
26
|
+
* `v3_*` out-parameter helpers writing into module scratch — no per-call arrays
|
|
27
|
+
* or face objects. The pools are module-level scratch reused across calls; the
|
|
28
|
+
* engine is single-threaded and GJK/EPA is never re-entered, matching the rest
|
|
29
|
+
* of the physics core's scratch convention.
|
|
21
30
|
*
|
|
22
31
|
* @author Alex Goldring
|
|
23
32
|
* @copyright Company Named Limited (c) 2026
|
|
@@ -26,25 +35,55 @@
|
|
|
26
35
|
const GJK_MAX_ITER = 64;
|
|
27
36
|
const EPA_MAX_ITER = 64;
|
|
28
37
|
const EPA_EPS = 1e-8;
|
|
38
|
+
const DIR_EPS = 1e-18;
|
|
39
|
+
|
|
40
|
+
// EPA pool capacities. EPA adds at most one vertex per iteration, so vertices are
|
|
41
|
+
// bounded by 4 + EPA_MAX_ITER (= 68), faces by Euler's 2V−4 (= 132) and horizon
|
|
42
|
+
// edges by 3V−6 (= 198). These caps clear those bounds with margin, so for the
|
|
43
|
+
// convex inputs the narrowphase feeds they are never reached; the guards below
|
|
44
|
+
// degrade to "use the current best face" purely as a defence against degenerate
|
|
45
|
+
// input rather than growing the pools.
|
|
46
|
+
const VERT_CAP = 128;
|
|
47
|
+
const FACE_CAP = 256;
|
|
48
|
+
const EDGE_CAP = 256;
|
|
29
49
|
|
|
50
|
+
// --- support scratch ---
|
|
30
51
|
const _sA = new Float64Array(3);
|
|
31
52
|
const _sB = new Float64Array(3);
|
|
53
|
+
const _w = new Float64Array(3); // newly sampled support point
|
|
32
54
|
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
55
|
+
// --- GJK simplex + search direction ---
|
|
56
|
+
const SIMPLEX = new Float64Array(12); // 4 points, 3 floats each; slot 0 = newest (a)
|
|
57
|
+
const DIR = new Float64Array(3);
|
|
58
|
+
|
|
59
|
+
// --- vector scratch (shared; GJK and EPA never run concurrently) ---
|
|
60
|
+
const _ab = new Float64Array(3);
|
|
61
|
+
const _ac = new Float64Array(3);
|
|
62
|
+
const _ad = new Float64Array(3);
|
|
63
|
+
const _abc = new Float64Array(3);
|
|
64
|
+
const _acd = new Float64Array(3);
|
|
65
|
+
const _adb = new Float64Array(3);
|
|
66
|
+
const _t = new Float64Array(3);
|
|
67
|
+
|
|
68
|
+
// --- EPA polytope pools ---
|
|
69
|
+
const VERTS = new Float64Array(VERT_CAP * 3);
|
|
70
|
+
const FACE_IDX = new Int32Array(FACE_CAP * 3); // vertex indices, 3 per face
|
|
71
|
+
const FACE_N = new Float64Array(FACE_CAP * 3); // outward unit normal, 3 per face
|
|
72
|
+
const FACE_DIST = new Float64Array(FACE_CAP); // perpendicular distance from origin
|
|
73
|
+
const EDGE = new Int32Array(EDGE_CAP * 2); // horizon edges, 2 vertex indices each
|
|
74
|
+
const CEN = new Float64Array(3); // polytope centroid (for outward orientation)
|
|
45
75
|
|
|
46
76
|
/**
|
|
47
|
-
*
|
|
77
|
+
* A convex shape exposing a world-space support mapping: `support(out, off, dx,
|
|
78
|
+
* dy, dz)` writes the farthest point of the shape in direction `(dx, dy, dz)`
|
|
79
|
+
* into `out[off..off+2]` (e.g. {@link PosedShape}).
|
|
80
|
+
*
|
|
81
|
+
* @typedef {{ support: function(Float64Array, number, number, number, number): void }} SupportProvider
|
|
82
|
+
*/
|
|
83
|
+
|
|
84
|
+
/**
|
|
85
|
+
* Minkowski-difference support written to `out[off..off+2]`:
|
|
86
|
+
* support_A(d̂) − support_B(−d̂), world space.
|
|
48
87
|
*
|
|
49
88
|
* The direction is normalised first: the support contract assumes a UNIT vector
|
|
50
89
|
* — `SphereShape3D`/`CapsuleShape3D` scale the raw direction by their radius
|
|
@@ -53,188 +92,441 @@ function tripleProduct(a, b, c) {
|
|
|
53
92
|
* simplex evolution). Polytope supports are scale-invariant (vertex argmax), so
|
|
54
93
|
* this only matters for curved providers, but it must hold for them. Matches the
|
|
55
94
|
* sibling `minkowski_support.js`, which normalises for the same reason.
|
|
95
|
+
*
|
|
96
|
+
* @param {Float64Array|number[]} out destination for the CSO point
|
|
97
|
+
* @param {number} off offset into `out`
|
|
98
|
+
* @param {SupportProvider} A first support provider
|
|
99
|
+
* @param {SupportProvider} B second support provider
|
|
100
|
+
* @param {number} dx search-direction x (need not be unit length)
|
|
101
|
+
* @param {number} dy search-direction y
|
|
102
|
+
* @param {number} dz search-direction z
|
|
103
|
+
* @returns {void}
|
|
56
104
|
*/
|
|
57
|
-
function
|
|
58
|
-
let dx = d[0], dy = d[1], dz = d[2];
|
|
105
|
+
function cso_support(out, off, A, B, dx, dy, dz) {
|
|
59
106
|
const len2 = dx * dx + dy * dy + dz * dz;
|
|
60
107
|
if (len2 > 1e-30) {
|
|
61
108
|
const inv = 1 / Math.sqrt(len2);
|
|
62
|
-
dx *= inv;
|
|
109
|
+
dx *= inv;
|
|
110
|
+
dy *= inv;
|
|
111
|
+
dz *= inv;
|
|
63
112
|
}
|
|
64
113
|
A.support(_sA, 0, dx, dy, dz);
|
|
65
114
|
B.support(_sB, 0, -dx, -dy, -dz);
|
|
66
|
-
|
|
115
|
+
out[off] = _sA[0] - _sB[0];
|
|
116
|
+
out[off + 1] = _sA[1] - _sB[1];
|
|
117
|
+
out[off + 2] = _sA[2] - _sB[2];
|
|
67
118
|
}
|
|
68
119
|
|
|
69
120
|
/**
|
|
70
|
-
* GJK intersection.
|
|
71
|
-
*
|
|
72
|
-
* point throughout (Muratori convention).
|
|
121
|
+
* GJK intersection. Fills {@link SIMPLEX} with a rank-4 simplex enclosing the
|
|
122
|
+
* origin and returns 4 on overlap; returns 0 if the shapes are separated.
|
|
123
|
+
* `SIMPLEX` slot 0 is the most-recent point throughout (Muratori convention).
|
|
124
|
+
*
|
|
125
|
+
* @param {SupportProvider} A first support provider
|
|
126
|
+
* @param {SupportProvider} B second support provider
|
|
127
|
+
* @returns {number} 4 if the origin is enclosed (shapes overlap), 0 if separated
|
|
73
128
|
*/
|
|
74
129
|
function gjk(A, B) {
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
79
|
-
|
|
130
|
+
cso_support(SIMPLEX, 0, A, B, 1, 0, 0);
|
|
131
|
+
if (v3_length_sqr(SIMPLEX[0], SIMPLEX[1], SIMPLEX[2]) < DIR_EPS) {
|
|
132
|
+
cso_support(SIMPLEX, 0, A, B, 0, 1, 0);
|
|
133
|
+
}
|
|
134
|
+
DIR[0] = -SIMPLEX[0];
|
|
135
|
+
DIR[1] = -SIMPLEX[1];
|
|
136
|
+
DIR[2] = -SIMPLEX[2];
|
|
137
|
+
let count = 1;
|
|
80
138
|
|
|
81
139
|
for (let iter = 0; iter < GJK_MAX_ITER; iter++) {
|
|
82
|
-
if (
|
|
83
|
-
|
|
84
|
-
|
|
85
|
-
|
|
86
|
-
if (
|
|
140
|
+
if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
|
|
141
|
+
return 0; // search direction collapsed → touching/separated
|
|
142
|
+
}
|
|
143
|
+
cso_support(_w, 0, A, B, DIR[0], DIR[1], DIR[2]);
|
|
144
|
+
if (v3_dot(_w[0], _w[1], _w[2], DIR[0], DIR[1], DIR[2]) < 0) {
|
|
145
|
+
return 0; // farthest point short of the origin → separated
|
|
146
|
+
}
|
|
147
|
+
// push the new point to slot 0, shifting the existing points down one slot
|
|
148
|
+
// (copyWithin is memmove-style: the overlapping forward copy is well-defined)
|
|
149
|
+
SIMPLEX.copyWithin(3, 0, count * 3);
|
|
150
|
+
SIMPLEX[0] = _w[0];
|
|
151
|
+
SIMPLEX[1] = _w[1];
|
|
152
|
+
SIMPLEX[2] = _w[2];
|
|
153
|
+
count++;
|
|
154
|
+
|
|
155
|
+
count = do_simplex(count);
|
|
156
|
+
if (count === 0) {
|
|
157
|
+
return 4; // origin enclosed by the tetrahedron
|
|
158
|
+
}
|
|
87
159
|
}
|
|
88
|
-
return
|
|
160
|
+
return 0;
|
|
89
161
|
}
|
|
90
162
|
|
|
91
163
|
/**
|
|
92
|
-
* Evolve the simplex toward the origin.
|
|
93
|
-
* the next search direction into
|
|
94
|
-
* encloses the origin.
|
|
164
|
+
* Evolve the simplex toward the origin. Reorders {@link SIMPLEX} in place and
|
|
165
|
+
* writes the next search direction into {@link DIR}. Returns the new simplex size,
|
|
166
|
+
* or 0 when a tetrahedron encloses the origin.
|
|
167
|
+
*
|
|
168
|
+
* @param {number} count current simplex size (2, 3, or 4)
|
|
169
|
+
* @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
|
|
95
170
|
*/
|
|
96
|
-
function
|
|
97
|
-
if (
|
|
98
|
-
|
|
99
|
-
|
|
171
|
+
function do_simplex(count) {
|
|
172
|
+
if (count === 2) {
|
|
173
|
+
return line_case();
|
|
174
|
+
}
|
|
175
|
+
if (count === 3) {
|
|
176
|
+
return triangle_case();
|
|
177
|
+
}
|
|
178
|
+
return tetra_case();
|
|
100
179
|
}
|
|
101
180
|
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
105
|
-
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
|
|
109
|
-
|
|
110
|
-
|
|
111
|
-
|
|
112
|
-
|
|
113
|
-
|
|
114
|
-
|
|
115
|
-
|
|
116
|
-
|
|
181
|
+
/**
|
|
182
|
+
* Line (2-point) simplex sub-case: pick the Voronoi region of segment [a, b] the
|
|
183
|
+
* origin projects into and set the next search direction in {@link DIR}.
|
|
184
|
+
*
|
|
185
|
+
* @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
|
|
186
|
+
*/
|
|
187
|
+
function line_case() {
|
|
188
|
+
const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
|
|
189
|
+
const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
|
|
190
|
+
v3_subtract(_ab, 0, bx, by, bz, ax, ay, az); // ab = b − a
|
|
191
|
+
const aox = -ax, aoy = -ay, aoz = -az; // ao = −a
|
|
192
|
+
|
|
193
|
+
if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
|
|
194
|
+
v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz); // (ab×ao)×ab
|
|
195
|
+
if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
|
|
196
|
+
// origin on the line — pick any perpendicular to ab
|
|
197
|
+
if (Math.abs(_ab[0]) < 0.9) {
|
|
198
|
+
v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 1, 0, 0);
|
|
199
|
+
} else {
|
|
200
|
+
v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 0, 1, 0);
|
|
201
|
+
}
|
|
202
|
+
}
|
|
203
|
+
return 2; // keep [a, b]
|
|
117
204
|
}
|
|
118
|
-
|
|
205
|
+
DIR[0] = aox;
|
|
206
|
+
DIR[1] = aoy;
|
|
207
|
+
DIR[2] = aoz;
|
|
208
|
+
return 1; // keep [a]
|
|
119
209
|
}
|
|
120
210
|
|
|
121
|
-
|
|
122
|
-
|
|
123
|
-
|
|
124
|
-
|
|
211
|
+
/**
|
|
212
|
+
* Triangle (3-point) simplex sub-case: classify the origin against triangle
|
|
213
|
+
* [a, b, c]'s edges and faces, reorder {@link SIMPLEX}, and set the next search
|
|
214
|
+
* direction in {@link DIR}.
|
|
215
|
+
*
|
|
216
|
+
* @returns {number} the new simplex size (1, 2, or 3)
|
|
217
|
+
*/
|
|
218
|
+
function triangle_case() {
|
|
219
|
+
const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
|
|
220
|
+
const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
|
|
221
|
+
const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
|
|
222
|
+
v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
|
|
223
|
+
v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
|
|
224
|
+
const aox = -ax, aoy = -ay, aoz = -az;
|
|
225
|
+
v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]); // abc = ab×ac
|
|
125
226
|
|
|
126
|
-
|
|
127
|
-
|
|
128
|
-
|
|
129
|
-
|
|
130
|
-
|
|
227
|
+
// outside edge ac? (abc×ac) · ao > 0
|
|
228
|
+
v3_cross(_t, 0, _abc[0], _abc[1], _abc[2], _ac[0], _ac[1], _ac[2]);
|
|
229
|
+
if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
|
|
230
|
+
if (v3_dot(_ac[0], _ac[1], _ac[2], aox, aoy, aoz) > 0) {
|
|
231
|
+
// keep [a, c]: move c into slot 1
|
|
232
|
+
v3_array_copy(SIMPLEX, 3, SIMPLEX, 6);
|
|
233
|
+
v3_triple_cross_product(DIR, 0, _ac[0], _ac[1], _ac[2], aox, aoy, aoz); // (ac×ao)×ac
|
|
234
|
+
return 2;
|
|
131
235
|
}
|
|
132
|
-
return
|
|
236
|
+
return star_case();
|
|
133
237
|
}
|
|
134
|
-
if (dot(cross(ab, abc), ao) > 0) return starCase(simplex, a, b, ao, dOut);
|
|
135
238
|
|
|
136
|
-
//
|
|
137
|
-
|
|
138
|
-
|
|
139
|
-
|
|
140
|
-
}
|
|
239
|
+
// outside edge ab? (ab×abc) · ao > 0
|
|
240
|
+
v3_cross(_t, 0, _ab[0], _ab[1], _ab[2], _abc[0], _abc[1], _abc[2]);
|
|
241
|
+
if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
|
|
242
|
+
return star_case();
|
|
243
|
+
}
|
|
141
244
|
|
|
142
|
-
|
|
143
|
-
|
|
144
|
-
|
|
145
|
-
|
|
146
|
-
|
|
245
|
+
// origin within the triangle's prism — above or below the face
|
|
246
|
+
if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
|
|
247
|
+
DIR[0] = _abc[0];
|
|
248
|
+
DIR[1] = _abc[1];
|
|
249
|
+
DIR[2] = _abc[2];
|
|
147
250
|
} else {
|
|
148
|
-
|
|
149
|
-
|
|
251
|
+
// below: reorder to [a, c, b] so the winding faces the origin; DIR = −abc
|
|
252
|
+
SIMPLEX[3] = cx;
|
|
253
|
+
SIMPLEX[4] = cy;
|
|
254
|
+
SIMPLEX[5] = cz;
|
|
255
|
+
SIMPLEX[6] = bx;
|
|
256
|
+
SIMPLEX[7] = by;
|
|
257
|
+
SIMPLEX[8] = bz;
|
|
258
|
+
DIR[0] = -_abc[0];
|
|
259
|
+
DIR[1] = -_abc[1];
|
|
260
|
+
DIR[2] = -_abc[2];
|
|
261
|
+
}
|
|
262
|
+
return 3; // keep the triangle
|
|
263
|
+
}
|
|
264
|
+
|
|
265
|
+
/**
|
|
266
|
+
* Edge region shared by the two triangle edges that fold back toward [a]: keep
|
|
267
|
+
* segment [a, b] or vertex [a] and set the next search direction in {@link DIR}.
|
|
268
|
+
*
|
|
269
|
+
* @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
|
|
270
|
+
*/
|
|
271
|
+
function star_case() {
|
|
272
|
+
const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
|
|
273
|
+
const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
|
|
274
|
+
v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
|
|
275
|
+
const aox = -ax, aoy = -ay, aoz = -az;
|
|
276
|
+
|
|
277
|
+
if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
|
|
278
|
+
v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz);
|
|
279
|
+
return 2; // keep [a, b]
|
|
280
|
+
}
|
|
281
|
+
DIR[0] = aox;
|
|
282
|
+
DIR[1] = aoy;
|
|
283
|
+
DIR[2] = aoz;
|
|
284
|
+
return 1; // keep [a]
|
|
285
|
+
}
|
|
286
|
+
|
|
287
|
+
/**
|
|
288
|
+
* Tetrahedron (4-point) simplex sub-case: if the origin lies outside one of the
|
|
289
|
+
* three faces incident to [a], reorder {@link SIMPLEX} to that triangle and
|
|
290
|
+
* recurse into {@link triangle_case}; otherwise the origin is enclosed.
|
|
291
|
+
*
|
|
292
|
+
* @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
|
|
293
|
+
*/
|
|
294
|
+
function tetra_case() {
|
|
295
|
+
const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
|
|
296
|
+
const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
|
|
297
|
+
const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
|
|
298
|
+
const dx = SIMPLEX[9], dy = SIMPLEX[10], dz = SIMPLEX[11];
|
|
299
|
+
v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
|
|
300
|
+
v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
|
|
301
|
+
v3_subtract(_ad, 0, dx, dy, dz, ax, ay, az);
|
|
302
|
+
const aox = -ax, aoy = -ay, aoz = -az;
|
|
303
|
+
v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]);
|
|
304
|
+
v3_cross(_acd, 0, _ac[0], _ac[1], _ac[2], _ad[0], _ad[1], _ad[2]);
|
|
305
|
+
v3_cross(_adb, 0, _ad[0], _ad[1], _ad[2], _ab[0], _ab[1], _ab[2]);
|
|
306
|
+
|
|
307
|
+
if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
|
|
308
|
+
// outside face abc → keep [a, b, c] (slots already correct)
|
|
309
|
+
return triangle_case();
|
|
310
|
+
}
|
|
311
|
+
if (v3_dot(_acd[0], _acd[1], _acd[2], aox, aoy, aoz) > 0) {
|
|
312
|
+
// outside face acd → reorder to [a, c, d]
|
|
313
|
+
SIMPLEX[3] = cx;
|
|
314
|
+
SIMPLEX[4] = cy;
|
|
315
|
+
SIMPLEX[5] = cz;
|
|
316
|
+
SIMPLEX[6] = dx;
|
|
317
|
+
SIMPLEX[7] = dy;
|
|
318
|
+
SIMPLEX[8] = dz;
|
|
319
|
+
return triangle_case();
|
|
320
|
+
}
|
|
321
|
+
if (v3_dot(_adb[0], _adb[1], _adb[2], aox, aoy, aoz) > 0) {
|
|
322
|
+
// outside face adb → reorder to [a, d, b]
|
|
323
|
+
SIMPLEX[3] = dx;
|
|
324
|
+
SIMPLEX[4] = dy;
|
|
325
|
+
SIMPLEX[5] = dz;
|
|
326
|
+
SIMPLEX[6] = bx;
|
|
327
|
+
SIMPLEX[7] = by;
|
|
328
|
+
SIMPLEX[8] = bz;
|
|
329
|
+
return triangle_case();
|
|
330
|
+
}
|
|
331
|
+
return 0; // origin inside the tetrahedron
|
|
332
|
+
}
|
|
333
|
+
|
|
334
|
+
/**
|
|
335
|
+
* Append a face spanning vertices (i, j, k) with an OUTWARD unit normal (away
|
|
336
|
+
* from the polytope centroid {@link CEN}); winding is flipped if needed to keep
|
|
337
|
+
* the normal outward.
|
|
338
|
+
*
|
|
339
|
+
* @param {number} i first vertex index into {@link VERTS}
|
|
340
|
+
* @param {number} j second vertex index into {@link VERTS}
|
|
341
|
+
* @param {number} k third vertex index into {@link VERTS}
|
|
342
|
+
* @param {number} face_count current number of faces (the new face's slot)
|
|
343
|
+
* @returns {number} the new face count
|
|
344
|
+
*/
|
|
345
|
+
function add_face(i, j, k, face_count) {
|
|
346
|
+
const ax = VERTS[i * 3], ay = VERTS[i * 3 + 1], az = VERTS[i * 3 + 2];
|
|
347
|
+
v3_subtract(_ab, 0, VERTS[j * 3], VERTS[j * 3 + 1], VERTS[j * 3 + 2], ax, ay, az);
|
|
348
|
+
v3_subtract(_ac, 0, VERTS[k * 3], VERTS[k * 3 + 1], VERTS[k * 3 + 2], ax, ay, az);
|
|
349
|
+
v3_cross(_t, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]);
|
|
350
|
+
|
|
351
|
+
let nx = _t[0], ny = _t[1], nz = _t[2];
|
|
352
|
+
const len = Math.sqrt(nx * nx + ny * ny + nz * nz) || 1;
|
|
353
|
+
const inv = 1 / len;
|
|
354
|
+
nx *= inv;
|
|
355
|
+
ny *= inv;
|
|
356
|
+
nz *= inv;
|
|
357
|
+
|
|
358
|
+
// orient outward: the normal must point away from the interior (centroid)
|
|
359
|
+
if (v3_dot(nx, ny, nz, ax - CEN[0], ay - CEN[1], az - CEN[2]) < 0) {
|
|
360
|
+
nx = -nx;
|
|
361
|
+
ny = -ny;
|
|
362
|
+
nz = -nz;
|
|
363
|
+
const swap = j;
|
|
364
|
+
j = k;
|
|
365
|
+
k = swap;
|
|
366
|
+
}
|
|
367
|
+
|
|
368
|
+
const fb = face_count * 3;
|
|
369
|
+
FACE_IDX[fb] = i;
|
|
370
|
+
FACE_IDX[fb + 1] = j;
|
|
371
|
+
FACE_IDX[fb + 2] = k;
|
|
372
|
+
FACE_N[fb] = nx;
|
|
373
|
+
FACE_N[fb + 1] = ny;
|
|
374
|
+
FACE_N[fb + 2] = nz;
|
|
375
|
+
FACE_DIST[face_count] = v3_dot(nx, ny, nz, ax, ay, az); // ≥ 0 once outward
|
|
376
|
+
return face_count + 1;
|
|
377
|
+
}
|
|
378
|
+
|
|
379
|
+
/**
|
|
380
|
+
* Add a horizon edge (i, j) with cancellation: if the reverse edge (j, i) is
|
|
381
|
+
* already present, both are interior to the visible region and drop out.
|
|
382
|
+
* Order is irrelevant to the horizon, so removal is an O(1) swap-with-last.
|
|
383
|
+
*
|
|
384
|
+
* @param {number} edge_count current number of horizon edges in {@link EDGE}
|
|
385
|
+
* @param {number} i edge start (vertex index)
|
|
386
|
+
* @param {number} j edge end (vertex index)
|
|
387
|
+
* @returns {number} the new edge count
|
|
388
|
+
*/
|
|
389
|
+
function add_edge(edge_count, i, j) {
|
|
390
|
+
for (let k = 0; k < edge_count; k++) {
|
|
391
|
+
if (EDGE[k * 2] === j && EDGE[k * 2 + 1] === i) {
|
|
392
|
+
const last = edge_count - 1;
|
|
393
|
+
EDGE[k * 2] = EDGE[last * 2];
|
|
394
|
+
EDGE[k * 2 + 1] = EDGE[last * 2 + 1];
|
|
395
|
+
return last;
|
|
396
|
+
}
|
|
150
397
|
}
|
|
151
|
-
|
|
398
|
+
if (edge_count >= EDGE_CAP) {
|
|
399
|
+
return edge_count; // horizon pool full (unreachable for convex inputs)
|
|
400
|
+
}
|
|
401
|
+
EDGE[edge_count * 2] = i;
|
|
402
|
+
EDGE[edge_count * 2 + 1] = j;
|
|
403
|
+
return edge_count + 1;
|
|
152
404
|
}
|
|
153
405
|
|
|
154
|
-
|
|
155
|
-
|
|
156
|
-
|
|
157
|
-
|
|
158
|
-
|
|
159
|
-
|
|
160
|
-
|
|
161
|
-
|
|
162
|
-
|
|
163
|
-
|
|
164
|
-
|
|
406
|
+
/**
|
|
407
|
+
* Copy a face record (vertex indices, normal, distance) from slot `src` to slot
|
|
408
|
+
* `dst` — used to compact surviving faces over removed ones.
|
|
409
|
+
*
|
|
410
|
+
* @param {number} dst destination face slot
|
|
411
|
+
* @param {number} src source face slot
|
|
412
|
+
* @returns {void}
|
|
413
|
+
*/
|
|
414
|
+
function copy_face(dst, src) {
|
|
415
|
+
const db = dst * 3, sb = src * 3;
|
|
416
|
+
FACE_IDX[db] = FACE_IDX[sb];
|
|
417
|
+
FACE_IDX[db + 1] = FACE_IDX[sb + 1];
|
|
418
|
+
FACE_IDX[db + 2] = FACE_IDX[sb + 2];
|
|
419
|
+
FACE_N[db] = FACE_N[sb];
|
|
420
|
+
FACE_N[db + 1] = FACE_N[sb + 1];
|
|
421
|
+
FACE_N[db + 2] = FACE_N[sb + 2];
|
|
422
|
+
FACE_DIST[dst] = FACE_DIST[src];
|
|
165
423
|
}
|
|
166
424
|
|
|
167
425
|
/**
|
|
168
|
-
* EPA over the Minkowski difference, seeded by
|
|
169
|
-
* Writes the unit penetration normal (B→A)
|
|
426
|
+
* EPA over the Minkowski difference, seeded by the rank-4 enclosing simplex left
|
|
427
|
+
* in {@link SIMPLEX} by {@link gjk}. Writes the unit penetration normal (B→A)
|
|
428
|
+
* into `out` and returns the depth.
|
|
429
|
+
*
|
|
430
|
+
* @param {SupportProvider} A first support provider
|
|
431
|
+
* @param {SupportProvider} B second support provider
|
|
432
|
+
* @param {Float64Array|number[]} out destination for the unit penetration normal (B→A)
|
|
433
|
+
* @returns {number} penetration depth, or 0 if the polytope degenerated
|
|
170
434
|
*/
|
|
171
|
-
function epa(A, B,
|
|
172
|
-
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
|
|
176
|
-
|
|
435
|
+
function epa(A, B, out) {
|
|
436
|
+
// seed the vertex pool with the simplex points
|
|
437
|
+
v3_array_copy(VERTS, 0, SIMPLEX, 0);
|
|
438
|
+
v3_array_copy(VERTS, 3, SIMPLEX, 3);
|
|
439
|
+
v3_array_copy(VERTS, 6, SIMPLEX, 6);
|
|
440
|
+
v3_array_copy(VERTS, 9, SIMPLEX, 9);
|
|
441
|
+
let vert_count = 4;
|
|
442
|
+
|
|
443
|
+
CEN[0] = (VERTS[0] + VERTS[3] + VERTS[6] + VERTS[9]) / 4;
|
|
444
|
+
CEN[1] = (VERTS[1] + VERTS[4] + VERTS[7] + VERTS[10]) / 4;
|
|
445
|
+
CEN[2] = (VERTS[2] + VERTS[5] + VERTS[8] + VERTS[11]) / 4;
|
|
177
446
|
|
|
178
|
-
let
|
|
447
|
+
let face_count = 0;
|
|
448
|
+
face_count = add_face(0, 1, 2, face_count);
|
|
449
|
+
face_count = add_face(0, 2, 3, face_count);
|
|
450
|
+
face_count = add_face(0, 3, 1, face_count);
|
|
451
|
+
face_count = add_face(1, 3, 2, face_count);
|
|
179
452
|
|
|
180
453
|
for (let iter = 0; iter < EPA_MAX_ITER; iter++) {
|
|
181
454
|
// closest face to the origin
|
|
182
|
-
let best =
|
|
183
|
-
for (let
|
|
184
|
-
|
|
185
|
-
|
|
186
|
-
|
|
187
|
-
|
|
188
|
-
|
|
189
|
-
const
|
|
190
|
-
|
|
191
|
-
|
|
192
|
-
|
|
193
|
-
|
|
194
|
-
|
|
195
|
-
|
|
196
|
-
|
|
197
|
-
|
|
198
|
-
|
|
199
|
-
|
|
200
|
-
|
|
455
|
+
let best = 0;
|
|
456
|
+
for (let f = 1; f < face_count; f++) {
|
|
457
|
+
if (FACE_DIST[f] < FACE_DIST[best]) {
|
|
458
|
+
best = f;
|
|
459
|
+
}
|
|
460
|
+
}
|
|
461
|
+
|
|
462
|
+
const bn = best * 3;
|
|
463
|
+
cso_support(_w, 0, A, B, FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2]);
|
|
464
|
+
const wdist = v3_dot(FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[best];
|
|
465
|
+
if (wdist < EPA_EPS) {
|
|
466
|
+
break; // converged onto the closest face
|
|
467
|
+
}
|
|
468
|
+
if (vert_count >= VERT_CAP) {
|
|
469
|
+
break; // vertex pool full (unreachable: EPA_MAX_ITER caps vertices at 68)
|
|
470
|
+
}
|
|
471
|
+
|
|
472
|
+
const wi = vert_count;
|
|
473
|
+
VERTS[wi * 3] = _w[0];
|
|
474
|
+
VERTS[wi * 3 + 1] = _w[1];
|
|
475
|
+
VERTS[wi * 3 + 2] = _w[2];
|
|
476
|
+
vert_count++;
|
|
477
|
+
|
|
478
|
+
// remove faces visible from w, compacting the survivors; collect the
|
|
479
|
+
// horizon (edges on exactly one removed face) with cancellation
|
|
480
|
+
let edge_count = 0;
|
|
481
|
+
let kept = 0;
|
|
482
|
+
for (let f = 0; f < face_count; f++) {
|
|
483
|
+
const fn = f * 3;
|
|
484
|
+
if (v3_dot(FACE_N[fn], FACE_N[fn + 1], FACE_N[fn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[f] > EPA_EPS) {
|
|
485
|
+
const i0 = FACE_IDX[fn], i1 = FACE_IDX[fn + 1], i2 = FACE_IDX[fn + 2];
|
|
486
|
+
edge_count = add_edge(edge_count, i0, i1);
|
|
487
|
+
edge_count = add_edge(edge_count, i1, i2);
|
|
488
|
+
edge_count = add_edge(edge_count, i2, i0);
|
|
489
|
+
} else {
|
|
490
|
+
if (kept !== f) {
|
|
491
|
+
copy_face(kept, f);
|
|
492
|
+
}
|
|
493
|
+
kept++;
|
|
494
|
+
}
|
|
495
|
+
}
|
|
496
|
+
face_count = kept;
|
|
497
|
+
|
|
498
|
+
for (let e = 0; e < edge_count; e++) {
|
|
499
|
+
if (face_count >= FACE_CAP) {
|
|
500
|
+
break; // face pool full (unreachable for convex inputs)
|
|
501
|
+
}
|
|
502
|
+
face_count = add_face(EDGE[e * 2], EDGE[e * 2 + 1], wi, face_count);
|
|
503
|
+
}
|
|
504
|
+
if (face_count === 0) {
|
|
505
|
+
break; // degenerate polytope (all faces visible + horizon cancelled)
|
|
201
506
|
}
|
|
202
|
-
faces = kept;
|
|
203
|
-
for (const [i, j] of edges) faces.push(makeFace(verts, [i, j, wi], cen));
|
|
204
|
-
if (faces.length === 0) break; // degenerate polytope (all faces visible + horizon cancelled)
|
|
205
507
|
}
|
|
206
508
|
|
|
207
|
-
|
|
208
|
-
|
|
209
|
-
//
|
|
210
|
-
//
|
|
211
|
-
|
|
212
|
-
out[
|
|
509
|
+
if (face_count === 0) {
|
|
510
|
+
// Degenerate: no face survived. Report no usable axis (the caller treats a
|
|
511
|
+
// non-positive depth as a miss) rather than reading a stale face and
|
|
512
|
+
// throwing out of the narrowphase step.
|
|
513
|
+
out[0] = 0;
|
|
514
|
+
out[1] = 0;
|
|
515
|
+
out[2] = 0;
|
|
213
516
|
return 0;
|
|
214
517
|
}
|
|
215
|
-
let best = faces[0];
|
|
216
|
-
for (let i = 1; i < faces.length; i++) if (faces[i].dist < best.dist) best = faces[i];
|
|
217
|
-
out[0] = best.n[0]; out[1] = best.n[1]; out[2] = best.n[2];
|
|
218
|
-
return best.dist;
|
|
219
|
-
}
|
|
220
518
|
|
|
221
|
-
|
|
222
|
-
|
|
223
|
-
|
|
224
|
-
|
|
225
|
-
|
|
226
|
-
n = [n[0] / l, n[1] / l, n[2] / l];
|
|
227
|
-
// orient outward: normal should point away from the polytope interior (centroid)
|
|
228
|
-
if (dot(n, sub(a, cen)) < 0) { n = neg(n); const t = idx[1]; idx = [idx[0], idx[2], t]; }
|
|
229
|
-
return { v: idx, n, dist: dot(n, a) }; // perpendicular distance from origin (≥0 once outward)
|
|
230
|
-
}
|
|
231
|
-
|
|
232
|
-
/** Add an edge with horizon cancellation: if the reverse edge is present, drop both. */
|
|
233
|
-
function addEdge(edges, i, j) {
|
|
234
|
-
for (let k = 0; k < edges.length; k++) {
|
|
235
|
-
if (edges[k][0] === j && edges[k][1] === i) { edges.splice(k, 1); return; }
|
|
519
|
+
let best = 0;
|
|
520
|
+
for (let f = 1; f < face_count; f++) {
|
|
521
|
+
if (FACE_DIST[f] < FACE_DIST[best]) {
|
|
522
|
+
best = f;
|
|
523
|
+
}
|
|
236
524
|
}
|
|
237
|
-
|
|
525
|
+
const bn = best * 3;
|
|
526
|
+
out[0] = FACE_N[bn];
|
|
527
|
+
out[1] = FACE_N[bn + 1];
|
|
528
|
+
out[2] = FACE_N[bn + 2];
|
|
529
|
+
return FACE_DIST[best];
|
|
238
530
|
}
|
|
239
531
|
|
|
240
532
|
/**
|
|
@@ -242,14 +534,15 @@ function addEdge(edges, i, j) {
|
|
|
242
534
|
* Writes the unit separation normal (B→A) into `out` and returns the depth
|
|
243
535
|
* (positive on overlap, 0 if separated — `out` untouched on 0).
|
|
244
536
|
*
|
|
245
|
-
* @param {Float64Array|number[]} out length ≥ 3
|
|
246
|
-
* @param {
|
|
247
|
-
* @param {
|
|
537
|
+
* @param {Float64Array|number[]} out destination for the unit normal, length ≥ 3
|
|
538
|
+
* @param {SupportProvider} A world-space support provider (e.g. PosedShape)
|
|
539
|
+
* @param {SupportProvider} B world-space support provider (e.g. PosedShape)
|
|
248
540
|
* @returns {number} penetration depth, or 0 if separated
|
|
249
541
|
*/
|
|
250
542
|
export function gjk_epa_penetration(out, A, B) {
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
543
|
+
if (gjk(A, B) < 4) {
|
|
544
|
+
return 0;
|
|
545
|
+
}
|
|
546
|
+
const depth = epa(A, B, out);
|
|
254
547
|
return depth > 0 && Number.isFinite(depth) ? depth : 0;
|
|
255
548
|
}
|