@woosh/meep-engine 2.163.8 → 2.163.9
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/2d/line/line_segment_intersection_fraction_2d.d.ts +4 -2
- package/src/core/geom/2d/line/line_segment_intersection_fraction_2d.d.ts.map +1 -1
- package/src/core/geom/2d/line/line_segment_intersection_fraction_2d.js +6 -4
- package/src/engine/navigation/mesh/NavigationMesh.d.ts +6 -0
- package/src/engine/navigation/mesh/NavigationMesh.d.ts.map +1 -1
- package/src/engine/navigation/mesh/NavigationMesh.js +145 -234
- package/src/engine/navigation/mesh/PATHFINDING_PLAN.md +61 -17
- package/src/engine/navigation/mesh/{navmesh_polyanya_find_path.d.ts → bt_mesh_face_find_path_polyanya.d.ts} +2 -2
- package/src/engine/navigation/mesh/bt_mesh_face_find_path_polyanya.d.ts.map +1 -0
- package/src/engine/navigation/mesh/{navmesh_polyanya_find_path.js → bt_mesh_face_find_path_polyanya.js} +117 -48
- package/src/engine/navigation/mesh/bvh_segment_penetrates_mesh.d.ts +21 -0
- package/src/engine/navigation/mesh/bvh_segment_penetrates_mesh.d.ts.map +1 -0
- package/src/engine/navigation/mesh/bvh_segment_penetrates_mesh.js +133 -0
- package/src/engine/navigation/mesh/navmesh_polyanya_find_path.d.ts.map +0 -1
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.163.
|
|
9
|
+
"version": "2.163.9",
|
|
10
10
|
"main": "build/meep.module.js",
|
|
11
11
|
"module": "build/meep.module.js",
|
|
12
12
|
"exports": {
|
|
@@ -1,7 +1,9 @@
|
|
|
1
1
|
/**
|
|
2
2
|
* Fraction `t` in [0,1] along the segment (s0 -> s1) at which the infinite line through (l0, l1)
|
|
3
|
-
* crosses it. Returns
|
|
4
|
-
* the segment) or the two are parallel/collinear.
|
|
3
|
+
* crosses it. Returns -1 when the segment lies entirely on one side of the line (no crossing within
|
|
4
|
+
* the segment) or the two are parallel/collinear. -1 is a value the valid fraction can never take (a
|
|
5
|
+
* real crossing is always in [0,1]), and it is used instead of NaN so callers reject the no-crossing
|
|
6
|
+
* case with a trivial comparison (`t < 0` / `=== -1`) rather than the slower Number.isNaN.
|
|
5
7
|
*
|
|
6
8
|
* Unlike {@link line_segment_compute_line_segment_intersection_array_2d}, the first pair of points
|
|
7
9
|
* defines an unbounded line (not a segment) and the result is the parametric position on the segment
|
|
@@ -1 +1 @@
|
|
|
1
|
-
{"version":3,"file":"line_segment_intersection_fraction_2d.d.ts","sourceRoot":"","sources":["../../../../../../src/core/geom/2d/line/line_segment_intersection_fraction_2d.js"],"names":[],"mappings":"AAEA
|
|
1
|
+
{"version":3,"file":"line_segment_intersection_fraction_2d.d.ts","sourceRoot":"","sources":["../../../../../../src/core/geom/2d/line/line_segment_intersection_fraction_2d.js"],"names":[],"mappings":"AAEA;;;;;;;;;;;;;;;;;;;;GAoBG;AACH,4DAVW,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,GACJ,MAAM,CAsBlB"}
|
|
@@ -2,8 +2,10 @@ import { v2_cross_product } from "../../vec2/v2_cross_product.js";
|
|
|
2
2
|
|
|
3
3
|
/**
|
|
4
4
|
* Fraction `t` in [0,1] along the segment (s0 -> s1) at which the infinite line through (l0, l1)
|
|
5
|
-
* crosses it. Returns
|
|
6
|
-
* the segment) or the two are parallel/collinear.
|
|
5
|
+
* crosses it. Returns -1 when the segment lies entirely on one side of the line (no crossing within
|
|
6
|
+
* the segment) or the two are parallel/collinear. -1 is a value the valid fraction can never take (a
|
|
7
|
+
* real crossing is always in [0,1]), and it is used instead of NaN so callers reject the no-crossing
|
|
8
|
+
* case with a trivial comparison (`t < 0` / `=== -1`) rather than the slower Number.isNaN.
|
|
7
9
|
*
|
|
8
10
|
* Unlike {@link line_segment_compute_line_segment_intersection_array_2d}, the first pair of points
|
|
9
11
|
* defines an unbounded line (not a segment) and the result is the parametric position on the segment
|
|
@@ -30,12 +32,12 @@ export function line_segment_intersection_fraction_2d(
|
|
|
30
32
|
const side_1 = v2_cross_product(dir_x, dir_y, s1_x - l0_x, s1_y - l0_y);
|
|
31
33
|
|
|
32
34
|
if ((side_0 > 0 && side_1 > 0) || (side_0 < 0 && side_1 < 0)) {
|
|
33
|
-
return
|
|
35
|
+
return -1; // both endpoints on the same side -> no crossing within the segment
|
|
34
36
|
}
|
|
35
37
|
|
|
36
38
|
const denom = side_0 - side_1;
|
|
37
39
|
if (denom === 0) {
|
|
38
|
-
return
|
|
40
|
+
return -1; // parallel / collinear
|
|
39
41
|
}
|
|
40
42
|
|
|
41
43
|
return side_0 / denom;
|
|
@@ -22,6 +22,12 @@ export class NavigationMesh {
|
|
|
22
22
|
* start and goal snapped onto the mesh surface (the closest walkable point to each query position),
|
|
23
23
|
* so they may differ from the raw `start_*`/`goal_*` inputs when those lie off the mesh.
|
|
24
24
|
*
|
|
25
|
+
* The path itself is the exact any-angle geodesic on the navmesh surface (Polyanya). It turns at
|
|
26
|
+
* obstacle corners and is subdivided wherever it crosses a face boundary, so every segment lies on a
|
|
27
|
+
* single triangle — it follows the surface exactly rather than flying over creases or tunnelling
|
|
28
|
+
* through folds. It is intrinsic to the surface — no global "up" is assumed — so it is correct on
|
|
29
|
+
* sloped and folded navmeshes as well as flat ones.
|
|
30
|
+
*
|
|
25
31
|
* @param {Float32Array} output packed XYZ triples
|
|
26
32
|
* @param {number} start_x
|
|
27
33
|
* @param {number} start_y
|
|
@@ -1 +1 @@
|
|
|
1
|
-
{"version":3,"file":"NavigationMesh.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/NavigationMesh.js"],"names":[],"mappings":"
|
|
1
|
+
{"version":3,"file":"NavigationMesh.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/NavigationMesh.js"],"names":[],"mappings":"AAyCA;IAEI,yBAAgC;IAEhC;;;OAGG;IACH,KAFU,GAAG,CAEG;IAEhB;;;;;;;;;OASG;IACH,0HARW,cAAc,QA8BxB;IAGD;;;;;;;;;;;;;;;;;;;;OAoBG;IACH,kBATW,YAAY,WACZ,MAAM,WACN,MAAM,WACN,MAAM,UACN,MAAM,UACN,MAAM,UACN,MAAM,GACJ,MAAM,CAqClB;CAEJ;+BA/I4C,gEAAgE;oBADzF,gCAAgC"}
|
|
@@ -1,234 +1,145 @@
|
|
|
1
|
-
import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
|
|
2
|
-
import { BinaryTopology, NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
|
|
3
|
-
import
|
|
4
|
-
import
|
|
5
|
-
import {
|
|
6
|
-
import {
|
|
7
|
-
import {
|
|
8
|
-
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
//
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
const
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
const
|
|
35
|
-
const
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
*
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
*
|
|
54
|
-
* @param {
|
|
55
|
-
* @param {
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
|
|
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
|
-
const
|
|
125
|
-
|
|
126
|
-
|
|
127
|
-
|
|
128
|
-
|
|
129
|
-
|
|
130
|
-
|
|
131
|
-
|
|
132
|
-
|
|
133
|
-
|
|
134
|
-
|
|
135
|
-
|
|
136
|
-
|
|
137
|
-
|
|
138
|
-
output
|
|
139
|
-
|
|
140
|
-
|
|
141
|
-
|
|
142
|
-
|
|
143
|
-
|
|
144
|
-
|
|
145
|
-
|
|
146
|
-
if (face_path_length === 0) {
|
|
147
|
-
// no face path exists (disconnected topology)
|
|
148
|
-
return 0;
|
|
149
|
-
}
|
|
150
|
-
|
|
151
|
-
// build portals
|
|
152
|
-
// ==================
|
|
153
|
-
|
|
154
|
-
// initialize vertex data pool, vertex index 0 = start, vertex index 1 = goal (snapped on-mesh)
|
|
155
|
-
scratch_vertices[0] = snapped_start_x;
|
|
156
|
-
scratch_vertices[1] = snapped_start_y;
|
|
157
|
-
scratch_vertices[2] = snapped_start_z;
|
|
158
|
-
|
|
159
|
-
scratch_vertices[3] = snapped_goal_x;
|
|
160
|
-
scratch_vertices[4] = snapped_goal_y;
|
|
161
|
-
scratch_vertices[5] = snapped_goal_z;
|
|
162
|
-
|
|
163
|
-
let next_vertex_index = 2;
|
|
164
|
-
|
|
165
|
-
// start portal, degenerate portal at the start point, exiting the start face
|
|
166
|
-
scratch_portal_vertices[0] = 0;
|
|
167
|
-
scratch_portal_vertices[1] = 0;
|
|
168
|
-
|
|
169
|
-
mesh.face_read_normal(scratch_portal_normals, 0, start_face_id);
|
|
170
|
-
|
|
171
|
-
let portal_index = 1;
|
|
172
|
-
|
|
173
|
-
// intermediate portals sit on the shared edge between two consecutive faces along the path;
|
|
174
|
-
// the edge is read in the winding order of the face we are exiting, giving a directed (left, right) pair
|
|
175
|
-
for (let i = 1; i < face_path_length; i++) {
|
|
176
|
-
|
|
177
|
-
const face_from = scratch_array_u32[i - 1];
|
|
178
|
-
const face_to = scratch_array_u32[i];
|
|
179
|
-
|
|
180
|
-
const loop = bt_faces_shared_loop(mesh, face_from, face_to);
|
|
181
|
-
|
|
182
|
-
const left_vertex = mesh.loop_read_vertex(loop);
|
|
183
|
-
const right_vertex = mesh.loop_read_vertex(mesh.loop_read_next(loop));
|
|
184
|
-
|
|
185
|
-
const left_index = next_vertex_index++;
|
|
186
|
-
const right_index = next_vertex_index++;
|
|
187
|
-
|
|
188
|
-
mesh.vertex_read_coordinate(scratch_vertices, left_index * 3, left_vertex);
|
|
189
|
-
mesh.vertex_read_coordinate(scratch_vertices, right_index * 3, right_vertex);
|
|
190
|
-
|
|
191
|
-
const portal_address = portal_index * 2;
|
|
192
|
-
scratch_portal_vertices[portal_address] = left_index;
|
|
193
|
-
scratch_portal_vertices[portal_address + 1] = right_index;
|
|
194
|
-
|
|
195
|
-
mesh.face_read_normal(scratch_portal_normals, portal_index * 3, face_from);
|
|
196
|
-
|
|
197
|
-
portal_index++;
|
|
198
|
-
}
|
|
199
|
-
|
|
200
|
-
// goal portal, degenerate at the goal point, uses the normal of the face containing the goal
|
|
201
|
-
const goal_portal_address = portal_index * 2;
|
|
202
|
-
|
|
203
|
-
scratch_portal_vertices[goal_portal_address] = 1;
|
|
204
|
-
scratch_portal_vertices[goal_portal_address + 1] = 1;
|
|
205
|
-
|
|
206
|
-
mesh.face_read_normal(scratch_portal_normals, portal_index * 3, goal_face_id);
|
|
207
|
-
|
|
208
|
-
portal_index++;
|
|
209
|
-
|
|
210
|
-
// execute string pulling
|
|
211
|
-
const path_vertex_count = funnel_string_pull(
|
|
212
|
-
scratch_array_u32,
|
|
213
|
-
0,
|
|
214
|
-
scratch_portal_vertices,
|
|
215
|
-
scratch_portal_normals,
|
|
216
|
-
portal_index,
|
|
217
|
-
scratch_vertices,
|
|
218
|
-
);
|
|
219
|
-
|
|
220
|
-
// build the final path
|
|
221
|
-
for (let i = 0; i < path_vertex_count; i++) {
|
|
222
|
-
const vertex_index = scratch_array_u32[i];
|
|
223
|
-
|
|
224
|
-
output[i * 3] = scratch_vertices[vertex_index * 3];
|
|
225
|
-
output[i * 3 + 1] = scratch_vertices[vertex_index * 3 + 1];
|
|
226
|
-
output[i * 3 + 2] = scratch_vertices[vertex_index * 3 + 2];
|
|
227
|
-
|
|
228
|
-
}
|
|
229
|
-
|
|
230
|
-
return path_vertex_count;
|
|
231
|
-
|
|
232
|
-
}
|
|
233
|
-
|
|
234
|
-
}
|
|
1
|
+
import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
|
|
2
|
+
import { BinaryTopology, NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
|
|
3
|
+
import Vector3 from "../../../core/geom/Vector3.js";
|
|
4
|
+
import { navmesh_build_topology } from "./build/navmesh_build_topology.js";
|
|
5
|
+
import { bt_mesh_face_find_path_polyanya } from "./bt_mesh_face_find_path_polyanya.js";
|
|
6
|
+
import { bvh_build_from_bt_mesh } from "./bvh_build_from_bt_mesh.js";
|
|
7
|
+
import { bvh_query_nearest_face } from "./bvh_query_nearest_face.js";
|
|
8
|
+
|
|
9
|
+
// snapped (on-mesh) start/goal positions, reused across queries
|
|
10
|
+
const scratch_start_point = new Float32Array(3);
|
|
11
|
+
const scratch_goal_point = new Float32Array(3);
|
|
12
|
+
|
|
13
|
+
const scratch_v0 = new Float32Array(3);
|
|
14
|
+
const scratch_v1 = new Float32Array(3);
|
|
15
|
+
const scratch_v2 = new Float32Array(3);
|
|
16
|
+
|
|
17
|
+
// Fraction of the way toward the face centroid to pull a query point. The Polyanya cone search
|
|
18
|
+
// degenerates when a root (the start) or the goal lies exactly on a triangle edge -- and snapped,
|
|
19
|
+
// grid-aligned queries frequently do. Nudging the point strictly into the face interior conditions the
|
|
20
|
+
// search; 1e-3 is far enough off any edge for the search tolerances yet visually still "the same point".
|
|
21
|
+
const FACE_INTERIOR_PULL = 1e-3;
|
|
22
|
+
|
|
23
|
+
/** Move `point` a small fraction toward the centroid of `face`, so it sits strictly inside the triangle. */
|
|
24
|
+
function nudge_into_face(mesh, face, point) {
|
|
25
|
+
const l0 = mesh.face_read_loop(face);
|
|
26
|
+
const l1 = mesh.loop_read_next(l0);
|
|
27
|
+
const l2 = mesh.loop_read_next(l1);
|
|
28
|
+
|
|
29
|
+
mesh.vertex_read_coordinate(scratch_v0, 0, mesh.loop_read_vertex(l0));
|
|
30
|
+
mesh.vertex_read_coordinate(scratch_v1, 0, mesh.loop_read_vertex(l1));
|
|
31
|
+
mesh.vertex_read_coordinate(scratch_v2, 0, mesh.loop_read_vertex(l2));
|
|
32
|
+
|
|
33
|
+
const cx = (scratch_v0[0] + scratch_v1[0] + scratch_v2[0]) / 3;
|
|
34
|
+
const cy = (scratch_v0[1] + scratch_v1[1] + scratch_v2[1]) / 3;
|
|
35
|
+
const cz = (scratch_v0[2] + scratch_v1[2] + scratch_v2[2]) / 3;
|
|
36
|
+
|
|
37
|
+
point[0] += (cx - point[0]) * FACE_INTERIOR_PULL;
|
|
38
|
+
point[1] += (cy - point[1]) * FACE_INTERIOR_PULL;
|
|
39
|
+
point[2] += (cz - point[2]) * FACE_INTERIOR_PULL;
|
|
40
|
+
}
|
|
41
|
+
|
|
42
|
+
export class NavigationMesh {
|
|
43
|
+
|
|
44
|
+
topology = new BinaryTopology();
|
|
45
|
+
|
|
46
|
+
/**
|
|
47
|
+
* Used for raycasts and neighborhood search.
|
|
48
|
+
* @type {BVH}
|
|
49
|
+
*/
|
|
50
|
+
bvh = new BVH();
|
|
51
|
+
|
|
52
|
+
/**
|
|
53
|
+
* Build from given scene geometry
|
|
54
|
+
* @param {BinaryTopology} source
|
|
55
|
+
* @param {number} [agent_radius]
|
|
56
|
+
* @param {number} [agent_height]
|
|
57
|
+
* @param {number} [agent_max_step_height] agent can bridge vertical gaps in topology, such as stepping up a stair
|
|
58
|
+
* @param {number} [agent_max_step_distance] agent can bridge lateral gaps in topology, such as stepping over a hole in the floor
|
|
59
|
+
* @param {number} [agent_max_climb_angle] In radians, how steep of an angle can the agent go up by
|
|
60
|
+
* @param {Vector3} [up] Defines world's "UP" direction, this is what the agent will respect for climbing constraint
|
|
61
|
+
*/
|
|
62
|
+
build({
|
|
63
|
+
source,
|
|
64
|
+
agent_radius = 0,
|
|
65
|
+
agent_height = 0,
|
|
66
|
+
agent_max_step_height = 0,
|
|
67
|
+
agent_max_step_distance = 0,
|
|
68
|
+
agent_max_climb_angle = Math.PI / 4,
|
|
69
|
+
up = Vector3.up,
|
|
70
|
+
}) {
|
|
71
|
+
|
|
72
|
+
navmesh_build_topology({
|
|
73
|
+
destination: this.topology,
|
|
74
|
+
source,
|
|
75
|
+
agent_radius,
|
|
76
|
+
agent_height,
|
|
77
|
+
agent_max_step_height,
|
|
78
|
+
agent_max_step_distance,
|
|
79
|
+
agent_max_climb_angle,
|
|
80
|
+
up,
|
|
81
|
+
});
|
|
82
|
+
|
|
83
|
+
bvh_build_from_bt_mesh(this.bvh, this.topology);
|
|
84
|
+
}
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
/**
|
|
88
|
+
* Compute a walkable path between the two points.
|
|
89
|
+
* The result is a sequence of 3d points written into `output`. The first and last points are the
|
|
90
|
+
* start and goal snapped onto the mesh surface (the closest walkable point to each query position),
|
|
91
|
+
* so they may differ from the raw `start_*`/`goal_*` inputs when those lie off the mesh.
|
|
92
|
+
*
|
|
93
|
+
* The path itself is the exact any-angle geodesic on the navmesh surface (Polyanya). It turns at
|
|
94
|
+
* obstacle corners and is subdivided wherever it crosses a face boundary, so every segment lies on a
|
|
95
|
+
* single triangle — it follows the surface exactly rather than flying over creases or tunnelling
|
|
96
|
+
* through folds. It is intrinsic to the surface — no global "up" is assumed — so it is correct on
|
|
97
|
+
* sloped and folded navmeshes as well as flat ones.
|
|
98
|
+
*
|
|
99
|
+
* @param {Float32Array} output packed XYZ triples
|
|
100
|
+
* @param {number} start_x
|
|
101
|
+
* @param {number} start_y
|
|
102
|
+
* @param {number} start_z
|
|
103
|
+
* @param {number} goal_x
|
|
104
|
+
* @param {number} goal_y
|
|
105
|
+
* @param {number} goal_z
|
|
106
|
+
* @returns {number} number of 3d points written to `output` (0 if no path was found)
|
|
107
|
+
*/
|
|
108
|
+
find_path(
|
|
109
|
+
output,
|
|
110
|
+
start_x, start_y, start_z,
|
|
111
|
+
goal_x, goal_y, goal_z
|
|
112
|
+
) {
|
|
113
|
+
|
|
114
|
+
const mesh = this.topology;
|
|
115
|
+
const bvh = this.bvh;
|
|
116
|
+
|
|
117
|
+
const start_face_id = bvh_query_nearest_face(bvh, mesh, start_x, start_y, start_z, scratch_start_point);
|
|
118
|
+
|
|
119
|
+
if (start_face_id === NULL_POINTER) {
|
|
120
|
+
// probably topology is empty
|
|
121
|
+
return 0;
|
|
122
|
+
}
|
|
123
|
+
|
|
124
|
+
const goal_face_id = bvh_query_nearest_face(bvh, mesh, goal_x, goal_y, goal_z, scratch_goal_point);
|
|
125
|
+
|
|
126
|
+
if (goal_face_id === NULL_POINTER) {
|
|
127
|
+
// should never happen if we got the start face
|
|
128
|
+
return 0;
|
|
129
|
+
}
|
|
130
|
+
|
|
131
|
+
// pull the snapped endpoints strictly inside their faces (see nudge_into_face)
|
|
132
|
+
nudge_into_face(mesh, start_face_id, scratch_start_point);
|
|
133
|
+
nudge_into_face(mesh, goal_face_id, scratch_goal_point);
|
|
134
|
+
|
|
135
|
+
// exact any-angle path between the query points snapped onto the walkable surface; returns 0 when
|
|
136
|
+
// the start and goal faces are in different connected components
|
|
137
|
+
return bt_mesh_face_find_path_polyanya(
|
|
138
|
+
output, mesh,
|
|
139
|
+
scratch_start_point[0], scratch_start_point[1], scratch_start_point[2], start_face_id,
|
|
140
|
+
scratch_goal_point[0], scratch_goal_point[1], scratch_goal_point[2], goal_face_id,
|
|
141
|
+
);
|
|
142
|
+
|
|
143
|
+
}
|
|
144
|
+
|
|
145
|
+
}
|
|
@@ -81,7 +81,7 @@ two far edges; visible pieces keep `root` (observable), pieces hidden behind a *
|
|
|
81
81
|
interval endpoint turn there (the corner becomes the new root, non-observable). It returns the exact
|
|
82
82
|
point path, turning only at obstacle corners — no corridor, no funnel post-pass.
|
|
83
83
|
|
|
84
|
-
Built as a **standalone 2-D module** `
|
|
84
|
+
Built as a **standalone 2-D module** `bt_mesh_face_find_path_polyanya` (xy-plane; not yet wired into the
|
|
85
85
|
3-D `NavigationMesh`). Two things were essential to get it correct *and* terminating:
|
|
86
86
|
- **Reflex-corner restriction:** only boundary vertices whose incident-triangle angles sum to > 180°
|
|
87
87
|
are turning roots. Admitting flat/convex boundary vertices makes the search blow up combinatorially.
|
|
@@ -101,8 +101,8 @@ Built as a **standalone 2-D module** `navmesh_polyanya_find_path` (xy-plane; not
|
|
|
101
101
|
(p50 36, p99 126, min 0.17 for local queries), ~24 searches/s — **~64× faster**. `shortest_path.spec`
|
|
102
102
|
and all 92 navigation tests stay green; the gradient trace + graph-walk fallback are kept, with a
|
|
103
103
|
full-field re-solve as the backstop for under-resolved hole-winding corridors.
|
|
104
|
-
3. **[done]** Phase 2. Standalone exact `
|
|
105
|
-
`
|
|
104
|
+
3. **[done]** Phase 2. Standalone exact `bt_mesh_face_find_path_polyanya` (2-D). Validated by
|
|
105
|
+
`bt_mesh_face_find_path_polyanya.spec.js`: exact on analytic oracles (straight line, single hole,
|
|
106
106
|
U-barrier, asymmetric wall — all corner-hugging to 4 decimals) and, over 60 random pairs on a holed
|
|
107
107
|
mesh, never longer than the FMM+funnel path and never shorter than the straight line (strictly
|
|
108
108
|
shorter on detours). Benchmark on the same ~100k-face mesh: **mean 7.1 ms/search** (p50 3.3, p90 5.5,
|
|
@@ -120,20 +120,21 @@ Built as a **standalone 2-D module** `navmesh_polyanya_find_path` (xy-plane; not
|
|
|
120
120
|
cost.
|
|
121
121
|
|
|
122
122
|
**GC-free node storage (done):** search nodes moved out of per-node object literals into fixed
|
|
123
|
-
16-word records in a `
|
|
124
|
-
`Uint32Heap4` (id + f32 score) instead of an object heap.
|
|
125
|
-
counter reset per query
|
|
126
|
-
|
|
127
|
-
|
|
128
|
-
re-read after a grow. This is a deliberate **latency**, not throughput, choice: it is a touch slower
|
|
123
|
+
16-word records in a flat `ArrayBuffer` (uint32 + float32 views overlaid), referenced by integer id,
|
|
124
|
+
with the open list a `Uint32Heap4` (id + f32 score) instead of an object heap. Allocation is a
|
|
125
|
+
monotonic bump counter reset per query; nodes are never released mid-search, so there is no free list
|
|
126
|
+
and no occupancy bookkeeping — the buffer only ever grows (by doubling, copying the records over) and
|
|
127
|
+
the two cached views are re-read after a grow. This is a deliberate **latency**, not throughput, choice: it is a touch slower
|
|
129
128
|
on mean throughput (**~4.7 → ~6.0 ms**, p99 6.8 → 18.3 on the bench — object literals are cheap and
|
|
130
129
|
the typical query is tiny, avg ~2 points, so there was little GC to remove and the typed-array
|
|
131
130
|
bookkeeping costs more than it saves *in aggregate*), but pathfinding is a hot path and the point is
|
|
132
131
|
to emit **zero garbage** so it never triggers a GC pause that jitters the rest of the engine's frame.
|
|
133
|
-
Unpredictable pauses are worse than a predictable constant cost here. (
|
|
134
|
-
|
|
132
|
+
Unpredictable pauses are worse than a predictable constant cost here. (Storage was first prototyped on
|
|
133
|
+
`BinaryElementPool`, but the never-free, bump-allocated access pattern reads none of its allocator —
|
|
134
|
+
no `allocate()`/`release()`/`is_allocated()`/`clear()` — so it was reduced to the raw `ArrayBuffer` it
|
|
135
|
+
degenerates to; the `allocate()`/`clear()` pool variants measured slower still.)
|
|
135
136
|
|
|
136
|
-
**3-D / intrinsic (done):** `
|
|
137
|
+
**3-D / intrinsic (done):** `bt_mesh_face_find_path_polyanya` takes 3-D start/goal and writes a 3-D point
|
|
137
138
|
path. It assumes **no global "up"** — the search follows the surface by UNFOLDING. Each node carries the
|
|
138
139
|
2-D positions of its entry edge in a frame accumulated along its own corridor; expanding flattens the
|
|
139
140
|
next triangle into that frame by placing the apex from its two 3-D edge lengths (an isometry of the
|
|
@@ -169,11 +170,54 @@ Built as a **standalone 2-D module** `navmesh_polyanya_find_path` (xy-plane; not
|
|
|
169
170
|
per-node work (an extra unfold + a 3-D touch) is minor by comparison. This is the price of "follow face
|
|
170
171
|
normals" with no up assumption, and it buys exactness on curved surfaces.
|
|
171
172
|
|
|
172
|
-
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
|
|
176
|
-
|
|
173
|
+
**Intermediate-node pruning — tried, does NOT pay off (two benches).** Implemented the Anya/Polyanya
|
|
174
|
+
pruning (collapse single-observable-successor "pass-through" nodes inline: no heap, no heuristic, no
|
|
175
|
+
alloc), exact and `spec`-green. On the open Eden-blob bench it was flat in the bulk (mean/p50/throughput
|
|
176
|
+
unchanged) with only a tail improvement (max 4.47 → 2.38 s); on a deliberately corridor-heavy mesh
|
|
177
|
+
(`bt_mesh_face_find_path_polyanya_rooms.bench.spec.js`, below) a deterministic same-queries measure was **identical**
|
|
178
|
+
(40 queries: 4261 ms baseline vs 4246 ms pruned). Root cause: the weak heuristic floods the *open
|
|
179
|
+
rooms*, where the cone fans into many successors; the narrow corridors where chaining fires are a small
|
|
180
|
+
fraction. Pruning attacks per-node overhead, but the bottleneck is the *node count* the heuristic
|
|
181
|
+
forces. Not committed.
|
|
182
|
+
|
|
183
|
+
**Rooms-and-corridors bench (`bt_mesh_face_find_path_polyanya_rooms.bench.spec.js`, `.skip`).** 25×25 square rooms,
|
|
184
|
+
each at one of 5 random heights, connected N/S/E/W by narrow ramped corridors (~99k faces, genuinely
|
|
185
|
+
non-planar). It dramatises the heuristic weakness: **mean ~2.4 s/search** (vs 9 ms on the open blob),
|
|
186
|
+
~0.4 searches/s, because the straight-3-D-chord estimate cuts through walls and across levels, so A*
|
|
187
|
+
explores most of the mesh. This is the standing stress case for any heuristic work.
|
|
188
|
+
|
|
189
|
+
**Wired into `NavigationMesh.find_path` (done).** It replaces the face-corridor + funnel string-pull:
|
|
190
|
+
snap start/goal via the BVH, then run Polyanya. One robustness measure was needed — the snapped points
|
|
191
|
+
are pulled a hair (1e-3) toward their face centroid, because the cone search degenerates when a root or
|
|
192
|
+
the goal lies exactly on a triangle edge, which snapped grid-aligned queries frequently do (the funnel
|
|
193
|
+
sidestepped it by orienting on face normals, not a cone). Full `navigation/mesh` suite stays green.
|
|
194
|
+
|
|
195
|
+
**Path validation + a finding (done).** Both benches keep their paths and validate them against a
|
|
196
|
+
BVH: `bvh_segment_penetrates_mesh` flags any piece that crosses a triangle interior transversally, and
|
|
197
|
+
sampling reports how far each piece strays off the surface. The finding: a **corner-only** waypoint path
|
|
198
|
+
is correct only AT its turning corners, so the straight chord between two corners on different levels
|
|
199
|
+
flew over / cut through the ramped geometry between them (the funnel had the same property) — on the
|
|
200
|
+
multi-level rooms bench ≈4 % of pieces penetrated, maxOff up to the full height range.
|
|
201
|
+
|
|
202
|
+
**Surface-following output (done — fixes the above).** The reconstruction no longer emits bare corner
|
|
203
|
+
waypoints; it emits the point where the geodesic crosses each face-boundary edge (`line(root→next) ∩
|
|
204
|
+
edge`, lifted to 3-D by lerping the edge endpoints), plus the turning corner at each root change. Every
|
|
205
|
+
emitted segment therefore lies within a single triangle, so the path neither flies over a convex crease
|
|
206
|
+
nor tunnels through a concave fold. Two subtleties: (1) the geodesic often grazes a shared grid vertex,
|
|
207
|
+
where `line ∩ edge` reports no crossing (-1, both endpoints on one side of the line) — snapped to the nearer endpoint;
|
|
208
|
+
(2) flat stretches would otherwise gain a point at every interior edge, so **collinear runs are collapsed
|
|
209
|
+
to their endpoints** (perpendicular offset / span < `COLLINEAR_EPS = 1e-4`). The flat-mesh output is thus
|
|
210
|
+
unchanged (minimal corner polyline — the sparse-output specs still hold); subdivision only adds points
|
|
211
|
+
where the surface actually folds. **Both benches now ASSERT `penetrating === 0 && offSurface === 0`**
|
|
212
|
+
(the rooms bench flipped from report to assert), and `bt_mesh_face_find_path_polyanya.spec.js` adds
|
|
213
|
+
minimal surface oracles: two triangles folded into a convex ridge and a concave valley, and a 3-triangle
|
|
214
|
+
folded strip — each asserts the path sticks to the surface (`maxOffSurface < 1e-3`) and bends over the
|
|
215
|
+
crease rather than chording across it.
|
|
216
|
+
|
|
217
|
+
Remaining lever: a tighter still-admissible heuristic is now clearly **the** win — the rooms bench shows
|
|
218
|
+
the chord heuristic is catastrophic on walled/multi-level meshes; candidates are a cached lower-bound
|
|
219
|
+
distance-to-goal field (one FMM-style sweep per query, like `bt_mesh_face_find_path`) or an
|
|
220
|
+
obstacle-aware estimate.
|
|
177
221
|
|
|
178
222
|
## Gates
|
|
179
223
|
|
|
@@ -13,5 +13,5 @@
|
|
|
13
13
|
* @param {number} goal_face triangle containing the goal point
|
|
14
14
|
* @returns {number} number of path POINTS written (3 numbers each), 0 if no path
|
|
15
15
|
*/
|
|
16
|
-
export function
|
|
17
|
-
//# sourceMappingURL=
|
|
16
|
+
export function bt_mesh_face_find_path_polyanya(output: number[] | Float64Array | Float32Array, topology: BinaryTopology, sx: number, sy: number, sz: number, start_face: number, gx: number, gy: number, gz: number, goal_face: number): number;
|
|
17
|
+
//# sourceMappingURL=bt_mesh_face_find_path_polyanya.d.ts.map
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
{"version":3,"file":"bt_mesh_face_find_path_polyanya.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/bt_mesh_face_find_path_polyanya.js"],"names":[],"mappings":"AA2TA;;;;;;;;;;;;;;GAcG;AACH,wDAZW,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,gCAElC,MAAM,MACN,MAAM,MACN,MAAM,cACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,aACN,MAAM,GACJ,MAAM,CA+HlB"}
|
|
@@ -1,7 +1,6 @@
|
|
|
1
1
|
import { Uint32Heap4 } from "../../../core/collection/heap/Uint32Heap4.js";
|
|
2
2
|
import { line_segment_intersection_fraction_2d } from "../../../core/geom/2d/line/line_segment_intersection_fraction_2d.js";
|
|
3
3
|
import { triangle2d_compute_area } from "../../../core/geom/2d/triangle2d_compute_area.js";
|
|
4
|
-
import { BinaryElementPool } from "../../../core/geom/3d/topology/struct/binary/BinaryElementPool.js";
|
|
5
4
|
import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
|
|
6
5
|
import { v2_distance } from "../../../core/geom/vec2/v2_distance.js";
|
|
7
6
|
|
|
@@ -28,13 +27,18 @@ import { v2_distance } from "../../../core/geom/vec2/v2_distance.js";
|
|
|
28
27
|
* shortest root->interval->goal measured with straight 3D chords (a lower bound on the geodesic, hence
|
|
29
28
|
* admissible); the terminal node's *exact* cost unfolds the goal into the node's frame instead.
|
|
30
29
|
*
|
|
31
|
-
* The output path
|
|
32
|
-
*
|
|
33
|
-
*
|
|
30
|
+
* The output path FOLLOWS THE SURFACE: it is subdivided at every face-boundary edge the geodesic
|
|
31
|
+
* crosses, so each emitted segment lies within a single triangle -- it never flies over a convex crease
|
|
32
|
+
* nor tunnels through a concave fold. Turning corners are mesh vertices (exact positions); each edge
|
|
33
|
+
* crossing is the exact line-vs-edge intersection lifted to 3D by lerping the edge's two endpoints; the
|
|
34
|
+
* start/goal are the given 3D points. Collinear runs (a flat stretch with no real bend) are collapsed to
|
|
35
|
+
* their endpoints, so on a planar mesh the output is just the minimal corner polyline -- the subdivision
|
|
36
|
+
* only adds points where the surface actually folds. Wired into NavigationMesh.find_path.
|
|
34
37
|
*/
|
|
35
38
|
|
|
36
39
|
const EPS = 1e-9; // area-sign slack (cone inside/outside tests)
|
|
37
40
|
const DEDUP_EPS = 1e-7; // merge consecutive output points equal to within f32 reconstruction error
|
|
41
|
+
const COLLINEAR_EPS = 1e-4; // drop a path point whose perpendicular offset from its neighbours / span is below this
|
|
38
42
|
|
|
39
43
|
// Parameter-space tolerance for edge fractions, interval coverage and vertex snapping. Vertices are
|
|
40
44
|
// stored as float32, so a point that is geometrically on an edge endpoint or already-covered carries
|
|
@@ -66,7 +70,7 @@ function touch_distance(rx, ry, i0x, i0y, i1x, i1y, gx, gy) {
|
|
|
66
70
|
tx = _reflect[0]; ty = _reflect[1];
|
|
67
71
|
}
|
|
68
72
|
const t = line_segment_intersection_fraction_2d(rx, ry, tx, ty, i0x, i0y, i1x, i1y);
|
|
69
|
-
if (
|
|
73
|
+
if (t >= -PEPS && t <= 1 + PEPS) { // -1 (no crossing) falls below -PEPS, so the range test rejects it
|
|
70
74
|
return v2_distance(rx, ry, tx, ty);
|
|
71
75
|
}
|
|
72
76
|
return Math.min(
|
|
@@ -248,19 +252,20 @@ function is_corner(topology, vid) {
|
|
|
248
252
|
|
|
249
253
|
// ---- node storage ----------------------------------------------------------------------------------
|
|
250
254
|
|
|
251
|
-
// Search nodes
|
|
255
|
+
// Search nodes are fixed 16-word records packed into one flat ArrayBuffer, addressed by id*NODE_WORDS,
|
|
252
256
|
// so the open list is a Uint32Heap4 (id + score) rather than an object heap -- NO per-node object
|
|
253
257
|
// allocation, so the hot path produces no garbage and never triggers a GC pause (which would otherwise
|
|
254
|
-
// jitter the rest of the engine).
|
|
255
|
-
//
|
|
256
|
-
//
|
|
257
|
-
//
|
|
258
|
+
// jitter the rest of the engine). Allocation is a monotonic bump counter reset per query; nodes are
|
|
259
|
+
// never released mid-query, so there is no free list and no occupancy bookkeeping. The buffer only ever
|
|
260
|
+
// grows (by doubling, copying the existing records over) and the two typed-array views are re-cached
|
|
261
|
+
// after a grow. (A pooled allocator with a free list + occupancy bitset degenerates to exactly this
|
|
262
|
+
// under a never-free, bump-allocated access pattern, so a raw buffer is the honest, lighter shape.)
|
|
258
263
|
//
|
|
259
264
|
// A node stores its entry edge as vertex ids (E0V,E1V) AND its 2D positions in the node's unfolded frame
|
|
260
265
|
// (E0X,E0Y,E1X,E1Y), the root (ROOTVID + RX,RY in the frame; ROOTVID==NULL_ID is the start point), and
|
|
261
266
|
// the interval as the fractions [T0,T1] along the edge. 3D is recovered at reconstruction from vertex
|
|
262
267
|
// ids; a terminal node stores the goal edge (E0V,E1V) and the final bend's fraction along it (N_FCT,
|
|
263
|
-
//
|
|
268
|
+
// -1 == a straight shot, since a real bend fraction is in [0,1]).
|
|
264
269
|
const N_FACE = 0, N_E0V = 1, N_E1V = 2;
|
|
265
270
|
const N_E0X = 3, N_E0Y = 4, N_E1X = 5, N_E1Y = 6;
|
|
266
271
|
const N_ROOTVID = 7, N_RX = 8, N_RY = 9;
|
|
@@ -269,22 +274,23 @@ const N_G = 12, N_PARENT = 13, N_TERMINAL = 14, N_FCT = 15;
|
|
|
269
274
|
const NODE_WORDS = 16;
|
|
270
275
|
const NULL_ID = 0xFFFFFFFF;
|
|
271
276
|
|
|
272
|
-
const _node_pool = new BinaryElementPool(NODE_WORDS * 4, 1024);
|
|
273
277
|
const _open = new Uint32Heap4();
|
|
274
278
|
|
|
279
|
+
// node records: uint32 and float32 views overlaid on one ArrayBuffer (_node_df aliases _node_du's bytes)
|
|
280
|
+
let _node_capacity = 1024; // capacity in nodes; grows by doubling
|
|
281
|
+
let _node_du = new Uint32Array(_node_capacity * NODE_WORDS);
|
|
282
|
+
let _node_df = new Float32Array(_node_du.buffer);
|
|
275
283
|
let _node_count = 0;
|
|
276
|
-
let _node_capacity = _node_pool.capacity;
|
|
277
|
-
let _node_du = _node_pool.data_uint32;
|
|
278
|
-
let _node_df = _node_pool.data_float32;
|
|
279
284
|
|
|
280
285
|
/** Reserve the next node slot, growing (and re-caching the views) only when the buffer is full. */
|
|
281
286
|
function alloc_node() {
|
|
282
287
|
const id = _node_count++;
|
|
283
288
|
if (id >= _node_capacity) {
|
|
284
|
-
|
|
285
|
-
|
|
286
|
-
_node_du
|
|
287
|
-
|
|
289
|
+
_node_capacity *= 2;
|
|
290
|
+
const grown = new Uint32Array(_node_capacity * NODE_WORDS);
|
|
291
|
+
grown.set(_node_du); // copy the existing records into the larger buffer
|
|
292
|
+
_node_du = grown;
|
|
293
|
+
_node_df = new Float32Array(grown.buffer);
|
|
288
294
|
}
|
|
289
295
|
return id;
|
|
290
296
|
}
|
|
@@ -298,8 +304,8 @@ let _goal_v0 = 0, _goal_v1 = 0, _goal_v2 = 0;
|
|
|
298
304
|
let _goal_w0 = 0, _goal_w1 = 0, _goal_w2 = 0;
|
|
299
305
|
const _bw = new Float64Array(3);
|
|
300
306
|
|
|
301
|
-
//
|
|
302
|
-
const
|
|
307
|
+
// edge-crossing points (flat x,y,z triples) gathered along the parent chain during reconstruction
|
|
308
|
+
const _recon = [];
|
|
303
309
|
|
|
304
310
|
function goal_weight(vid) {
|
|
305
311
|
return vid === _goal_v0 ? _goal_w0 : (vid === _goal_v1 ? _goal_w1 : _goal_w2);
|
|
@@ -322,7 +328,7 @@ function goal_weight(vid) {
|
|
|
322
328
|
* @param {number} goal_face triangle containing the goal point
|
|
323
329
|
* @returns {number} number of path POINTS written (3 numbers each), 0 if no path
|
|
324
330
|
*/
|
|
325
|
-
export function
|
|
331
|
+
export function bt_mesh_face_find_path_polyanya(output, topology, sx, sy, sz, start_face, gx, gy, gz, goal_face) {
|
|
326
332
|
_sx = sx; _sy = sy; _sz = sz;
|
|
327
333
|
_gx = gx; _gy = gy; _gz = gz;
|
|
328
334
|
|
|
@@ -373,41 +379,101 @@ export function navmesh_polyanya_find_path(output, topology, sx, sy, sz, start_f
|
|
|
373
379
|
|
|
374
380
|
if (best === NULL_ID) return 0;
|
|
375
381
|
|
|
376
|
-
//
|
|
382
|
+
// Reconstruct a SURFACE-FOLLOWING path. Every node in the parent chain shares one continuous unfolded
|
|
383
|
+
// frame (each node's frame extends its parent's), so the whole route -- start, turning corners, goal --
|
|
384
|
+
// lays out as a single 2D polyline in that frame, and the point where the route crosses each face's
|
|
385
|
+
// entry edge is just the intersection of that polyline with the edge. Lifting each crossing back onto
|
|
386
|
+
// its 3D edge keeps every output segment inside one face (i.e. on the surface). Crossings that fall on
|
|
387
|
+
// a straight (flat) stretch collapse away in push_point, so flat meshes still yield minimal paths.
|
|
377
388
|
const du = _node_du, df = _node_df;
|
|
378
|
-
|
|
389
|
+
const bb = best * NODE_WORDS;
|
|
390
|
+
|
|
391
|
+
// the goal's position in the shared frame: unfold the goal-face apex from the terminal's entry edge
|
|
392
|
+
const t_e0v = du[bb + N_E0V], t_e1v = du[bb + N_E1V];
|
|
393
|
+
const t_e0x = df[bb + N_E0X], t_e0y = df[bb + N_E0Y], t_e1x = df[bb + N_E1X], t_e1y = df[bb + N_E1Y];
|
|
394
|
+
const g_apex = unfold_apex(topology, goal_face, t_e0v, t_e1v, t_e0x, t_e0y, t_e1x, t_e1y);
|
|
395
|
+
const goal_u = goal_weight(t_e0v) * t_e0x + goal_weight(t_e1v) * t_e1x + goal_weight(g_apex) * _apex[0];
|
|
396
|
+
const goal_v = goal_weight(t_e0v) * t_e0y + goal_weight(t_e1v) * t_e1y + goal_weight(g_apex) * _apex[1];
|
|
397
|
+
const t_fct = df[bb + N_FCT];
|
|
398
|
+
|
|
399
|
+
// walk the chain (goal end -> start), recording each entry-edge crossing in reverse path order
|
|
400
|
+
_recon.length = 0;
|
|
401
|
+
let next_u = goal_u, next_v = goal_v; // the next waypoint toward the goal, in the shared frame
|
|
402
|
+
let prev_root = du[bb + N_ROOTVID];
|
|
403
|
+
let prev_rx = df[bb + N_RX], prev_ry = df[bb + N_RY];
|
|
404
|
+
|
|
379
405
|
for (let n = best; n !== NULL_ID; n = du[n * NODE_WORDS + N_PARENT]) {
|
|
380
|
-
|
|
381
|
-
|
|
406
|
+
const base = n * NODE_WORDS;
|
|
407
|
+
const rootVid = du[base + N_ROOTVID];
|
|
408
|
+
const rx = df[base + N_RX], ry = df[base + N_RY];
|
|
409
|
+
|
|
410
|
+
// a change of root means the route turned at the previous corridor's root: emit that corner
|
|
411
|
+
// (an exact mesh vertex -- the turning point) and aim subsequent crossings at it
|
|
412
|
+
if (rootVid !== prev_root) {
|
|
413
|
+
if (prev_root !== NULL_ID) {
|
|
414
|
+
topology.vertex_read_coordinate(_va, 0, prev_root);
|
|
415
|
+
_recon.push(_va[0], _va[1], _va[2]);
|
|
416
|
+
}
|
|
417
|
+
next_u = prev_rx; next_v = prev_ry;
|
|
418
|
+
prev_root = rootVid;
|
|
419
|
+
}
|
|
382
420
|
|
|
383
|
-
|
|
384
|
-
|
|
385
|
-
|
|
386
|
-
|
|
387
|
-
|
|
421
|
+
const e0v = du[base + N_E0V], e1v = du[base + N_E1V];
|
|
422
|
+
const e0x = df[base + N_E0X], e0y = df[base + N_E0Y], e1x = df[base + N_E1X], e1y = df[base + N_E1Y];
|
|
423
|
+
|
|
424
|
+
// fraction along the entry edge where the route root->next crosses it (the terminal's goal-edge
|
|
425
|
+
// bend is exact and pre-computed as N_FCT)
|
|
426
|
+
let u;
|
|
427
|
+
if (n === best && t_fct !== -1) {
|
|
428
|
+
u = t_fct;
|
|
388
429
|
} else {
|
|
389
|
-
|
|
390
|
-
|
|
430
|
+
u = line_segment_intersection_fraction_2d(rx, ry, next_u, next_v, e0x, e0y, e1x, e1y);
|
|
431
|
+
if (u === -1) {
|
|
432
|
+
// the route line grazes a shared vertex (passes through / just outside an edge endpoint,
|
|
433
|
+
// common when the geodesic runs along a row of grid vertices): snap to the nearer endpoint
|
|
434
|
+
const dx = next_u - rx, dy = next_v - ry;
|
|
435
|
+
const s0 = Math.abs(dx * (e0y - ry) - dy * (e0x - rx));
|
|
436
|
+
const s1 = Math.abs(dx * (e1y - ry) - dy * (e1x - rx));
|
|
437
|
+
u = s0 <= s1 ? 0 : 1;
|
|
438
|
+
}
|
|
391
439
|
}
|
|
440
|
+
|
|
441
|
+
topology.vertex_read_coordinate(_va, 0, e0v);
|
|
442
|
+
topology.vertex_read_coordinate(_vb, 0, e1v);
|
|
443
|
+
_recon.push(_va[0] + (_vb[0] - _va[0]) * u, _va[1] + (_vb[1] - _va[1]) * u, _va[2] + (_vb[2] - _va[2]) * u);
|
|
444
|
+
|
|
445
|
+
prev_rx = rx; prev_ry = ry;
|
|
392
446
|
}
|
|
393
447
|
|
|
394
|
-
|
|
395
|
-
|
|
396
|
-
|
|
397
|
-
|
|
398
|
-
|
|
399
|
-
count = push_point(output, count,
|
|
400
|
-
_va[0] + (_vb[0] - _va[0]) * fct,
|
|
401
|
-
_va[1] + (_vb[1] - _va[1]) * fct,
|
|
402
|
-
_va[2] + (_vb[2] - _va[2]) * fct);
|
|
448
|
+
// start, the crossings in path order (reverse of how they were collected), goal
|
|
449
|
+
let count = 0;
|
|
450
|
+
count = push_point(output, count, _sx, _sy, _sz);
|
|
451
|
+
for (let i = _recon.length - 3; i >= 0; i -= 3) {
|
|
452
|
+
count = push_point(output, count, _recon[i], _recon[i + 1], _recon[i + 2]);
|
|
403
453
|
}
|
|
404
454
|
count = push_point(output, count, _gx, _gy, _gz);
|
|
405
455
|
return count / 3;
|
|
406
456
|
}
|
|
407
457
|
|
|
408
|
-
/**
|
|
458
|
+
/**
|
|
459
|
+
* Append a 3D point, collapsing collinear runs: if the previous point lies (within tolerance) on the
|
|
460
|
+
* segment between the one before it and the new point, it is redundant and gets replaced rather than
|
|
461
|
+
* kept. Flat stretches of a surface-following path thus reduce to their endpoints; genuine bends (turning
|
|
462
|
+
* corners, folds in the surface) survive.
|
|
463
|
+
*/
|
|
409
464
|
function push_point(output, count, x, y, z) {
|
|
410
|
-
if (count >=
|
|
465
|
+
if (count >= 6) {
|
|
466
|
+
const ax = output[count - 6], ay = output[count - 5], az = output[count - 4];
|
|
467
|
+
const bx = output[count - 3], by = output[count - 2], bz = output[count - 1];
|
|
468
|
+
const abx = bx - ax, aby = by - ay, abz = bz - az;
|
|
469
|
+
const acx = x - ax, acy = y - ay, acz = z - az;
|
|
470
|
+
const cross_x = aby * acz - abz * acy, cross_y = abz * acx - abx * acz, cross_z = abx * acy - aby * acx;
|
|
471
|
+
const ac_len = Math.hypot(acx, acy, acz);
|
|
472
|
+
if (ac_len < DEDUP_EPS || Math.hypot(cross_x, cross_y, cross_z) <= COLLINEAR_EPS * ac_len) {
|
|
473
|
+
output[count - 3] = x; output[count - 2] = y; output[count - 1] = z;
|
|
474
|
+
return count;
|
|
475
|
+
}
|
|
476
|
+
} else if (count >= 3 &&
|
|
411
477
|
Math.abs(output[count - 3] - x) <= DEDUP_EPS &&
|
|
412
478
|
Math.abs(output[count - 2] - y) <= DEDUP_EPS &&
|
|
413
479
|
Math.abs(output[count - 1] - z) <= DEDUP_EPS) {
|
|
@@ -459,8 +525,9 @@ function process_far_edge(topology, from_face, Pv, Px, Py, Qv, Qx, Qy, rx, ry, r
|
|
|
459
525
|
let m = 0;
|
|
460
526
|
const tR = line_segment_intersection_fraction_2d(rx, ry, i0x, i0y, Px, Py, Qx, Qy);
|
|
461
527
|
const tL = line_segment_intersection_fraction_2d(rx, ry, i1x, i1y, Px, Py, Qx, Qy);
|
|
462
|
-
|
|
463
|
-
if (
|
|
528
|
+
// -1 (no crossing) fails `> PEPS`, so the interior-fraction test rejects it without a NaN check
|
|
529
|
+
if (tR > PEPS && tR < 1 - PEPS) _split[m++] = tR;
|
|
530
|
+
if (tL > PEPS && tL < 1 - PEPS) _split[m++] = tL;
|
|
464
531
|
if (m === 2 && _split[0] > _split[1]) { const t = _split[0]; _split[0] = _split[1]; _split[1] = t; }
|
|
465
532
|
|
|
466
533
|
let prev = 0;
|
|
@@ -561,13 +628,13 @@ function emit_node(topology, neighbour, Pv, Qv, Px, Py, Qx, Qy, rx, ry, rootVid,
|
|
|
561
628
|
|
|
562
629
|
const fcost = g + touch_distance(rx, ry, i0x, i0y, i1x, i1y, ggx, ggy);
|
|
563
630
|
|
|
564
|
-
let fct =
|
|
631
|
+
let fct = -1; // -1 = straight shot (no forced bend at an interval endpoint); a real bend is in [0,1]
|
|
565
632
|
let tx = ggx, ty = ggy;
|
|
566
633
|
if (triangle2d_compute_area(i0x, i0y, i1x, i1y, rx, ry) * triangle2d_compute_area(i0x, i0y, i1x, i1y, ggx, ggy) > 0) {
|
|
567
634
|
reflect(ggx, ggy, i0x, i0y, i1x, i1y); tx = _reflect[0]; ty = _reflect[1];
|
|
568
635
|
}
|
|
569
636
|
const t = line_segment_intersection_fraction_2d(rx, ry, tx, ty, i0x, i0y, i1x, i1y);
|
|
570
|
-
if (
|
|
637
|
+
if (t < -PEPS || t > 1 + PEPS) { // -1 (no crossing) is caught by `< -PEPS`
|
|
571
638
|
const d0 = v2_distance(rx, ry, i0x, i0y) + v2_distance(i0x, i0y, ggx, ggy);
|
|
572
639
|
const d1 = v2_distance(rx, ry, i1x, i1y) + v2_distance(i1x, i1y, ggx, ggy);
|
|
573
640
|
fct = d0 <= d1 ? i0p : i1p;
|
|
@@ -579,6 +646,8 @@ function emit_node(topology, neighbour, Pv, Qv, Px, Py, Qx, Qy, rx, ry, rootVid,
|
|
|
579
646
|
du[b + N_TERMINAL] = 1;
|
|
580
647
|
du[b + N_ROOTVID] = rootVid === -1 ? NULL_ID : rootVid;
|
|
581
648
|
du[b + N_E0V] = Pv; du[b + N_E1V] = Qv;
|
|
649
|
+
df[b + N_E0X] = Px; df[b + N_E0Y] = Py; df[b + N_E1X] = Qx; df[b + N_E1Y] = Qy;
|
|
650
|
+
df[b + N_RX] = rx; df[b + N_RY] = ry;
|
|
582
651
|
du[b + N_PARENT] = parent;
|
|
583
652
|
df[b + N_FCT] = fct;
|
|
584
653
|
_open.insert(id, fcost);
|
|
@@ -0,0 +1,21 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Does the segment (ax,ay,az)->(bx,by,bz) pass THROUGH the mesh surface -- cross the interior of any
|
|
3
|
+
* triangle transversally (entering one side, exiting the other) strictly between its endpoints?
|
|
4
|
+
*
|
|
5
|
+
* This is the "penetration" test for a navmesh path piece. A piece that hugs the surface (coplanar with
|
|
6
|
+
* it) or flies over / under it -- a corner-only waypoint chord on a non-planar surface -- does NOT
|
|
7
|
+
* penetrate; a piece that tunnels through a wall or floor does. Endpoints lying on the surface (every
|
|
8
|
+
* path waypoint) are not penetrations.
|
|
9
|
+
*
|
|
10
|
+
* @param {BVH} bvh built by bvh_build_from_bt_mesh over `mesh`
|
|
11
|
+
* @param {BinaryTopology} mesh triangle mesh
|
|
12
|
+
* @param {number} ax
|
|
13
|
+
* @param {number} ay
|
|
14
|
+
* @param {number} az
|
|
15
|
+
* @param {number} bx
|
|
16
|
+
* @param {number} by
|
|
17
|
+
* @param {number} bz
|
|
18
|
+
* @returns {boolean}
|
|
19
|
+
*/
|
|
20
|
+
export function bvh_segment_penetrates_mesh(bvh: BVH, mesh: BinaryTopology, ax: number, ay: number, az: number, bx: number, by: number, bz: number): boolean;
|
|
21
|
+
//# sourceMappingURL=bvh_segment_penetrates_mesh.d.ts.map
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
{"version":3,"file":"bvh_segment_penetrates_mesh.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/bvh_segment_penetrates_mesh.js"],"names":[],"mappings":"AAgEA;;;;;;;;;;;;;;;;;;GAkBG;AACH,gFARW,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,GACJ,OAAO,CAmDnB"}
|
|
@@ -0,0 +1,133 @@
|
|
|
1
|
+
import {
|
|
2
|
+
COLUMN_CHILD_1,
|
|
3
|
+
COLUMN_CHILD_2,
|
|
4
|
+
COLUMN_USER_DATA,
|
|
5
|
+
ELEMENT_WORD_COUNT,
|
|
6
|
+
NULL_NODE
|
|
7
|
+
} from "../../../core/bvh2/bvh3/BVH.js";
|
|
8
|
+
import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
|
|
9
|
+
|
|
10
|
+
// Plane-side slack and barycentric slack. Mesh vertices are float32, so a segment endpoint that lies ON
|
|
11
|
+
// a triangle sits within ~coord*2^-23 of its plane; PLANE_EPS keeps those (and grazing/coplanar pieces)
|
|
12
|
+
// from registering as a crossing. Only a genuinely transversal piece -- strictly on opposite sides of a
|
|
13
|
+
// triangle's plane and crossing its interior away from its own endpoints -- counts as a penetration.
|
|
14
|
+
const PLANE_EPS = 1e-3;
|
|
15
|
+
const BARY_EPS = 1e-6;
|
|
16
|
+
|
|
17
|
+
const _coords = new Float32Array(3);
|
|
18
|
+
const _stack = new Uint32Array(1024); // DFS node stack; navmesh BVHs are shallow (depth ~ log2(faces))
|
|
19
|
+
|
|
20
|
+
/** Standard slab test: does the segment p + t*d, t in [0,1], intersect the AABB [min,max]? */
|
|
21
|
+
function segment_intersects_aabb(minx, miny, minz, maxx, maxy, maxz, px, py, pz, dx, dy, dz) {
|
|
22
|
+
let tmin = 0, tmax = 1;
|
|
23
|
+
|
|
24
|
+
if (dx > -1e-12 && dx < 1e-12) { if (px < minx || px > maxx) return false; }
|
|
25
|
+
else { let t1 = (minx - px) / dx, t2 = (maxx - px) / dx; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
|
|
26
|
+
|
|
27
|
+
if (dy > -1e-12 && dy < 1e-12) { if (py < miny || py > maxy) return false; }
|
|
28
|
+
else { let t1 = (miny - py) / dy, t2 = (maxy - py) / dy; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
|
|
29
|
+
|
|
30
|
+
if (dz > -1e-12 && dz < 1e-12) { if (pz < minz || pz > maxz) return false; }
|
|
31
|
+
else { let t1 = (minz - pz) / dz, t2 = (maxz - pz) / dz; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
|
|
32
|
+
|
|
33
|
+
return true;
|
|
34
|
+
}
|
|
35
|
+
|
|
36
|
+
/** True if segment (px,py,pz)->(qx,qy,qz) crosses the interior of triangle (a,b,c) transversally. */
|
|
37
|
+
function segment_crosses_triangle(px, py, pz, qx, qy, qz, ax, ay, az, bx, by, bz, cx, cy, cz) {
|
|
38
|
+
const ux = bx - ax, uy = by - ay, uz = bz - az;
|
|
39
|
+
const vx = cx - ax, vy = cy - ay, vz = cz - az;
|
|
40
|
+
const nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx;
|
|
41
|
+
const nlen = Math.hypot(nx, ny, nz);
|
|
42
|
+
if (nlen < 1e-12) return false; // degenerate triangle
|
|
43
|
+
|
|
44
|
+
const dP = ((px - ax) * nx + (py - ay) * ny + (pz - az) * nz) / nlen;
|
|
45
|
+
const dQ = ((qx - ax) * nx + (qy - ay) * ny + (qz - az) * nz) / nlen;
|
|
46
|
+
|
|
47
|
+
// must cross the plane strictly (not coplanar, not endpoint-on-plane)
|
|
48
|
+
if (!((dP > PLANE_EPS && dQ < -PLANE_EPS) || (dP < -PLANE_EPS && dQ > PLANE_EPS))) return false;
|
|
49
|
+
|
|
50
|
+
const t = dP / (dP - dQ);
|
|
51
|
+
if (t <= PLANE_EPS || t >= 1 - PLANE_EPS) return false; // crossing at/near an endpoint (on the surface)
|
|
52
|
+
|
|
53
|
+
// crossing point, tested for containment via barycentric coordinates in the triangle plane
|
|
54
|
+
const wx = px + (qx - px) * t - ax, wy = py + (qy - py) * t - ay, wz = pz + (qz - pz) * t - az;
|
|
55
|
+
const d00 = ux * ux + uy * uy + uz * uz, d01 = ux * vx + uy * vy + uz * vz, d11 = vx * vx + vy * vy + vz * vz;
|
|
56
|
+
const d20 = wx * ux + wy * uy + wz * uz, d21 = wx * vx + wy * vy + wz * vz;
|
|
57
|
+
const denom = d00 * d11 - d01 * d01;
|
|
58
|
+
if (denom > -1e-18 && denom < 1e-18) return false;
|
|
59
|
+
const s = (d11 * d20 - d01 * d21) / denom;
|
|
60
|
+
const w = (d00 * d21 - d01 * d20) / denom;
|
|
61
|
+
|
|
62
|
+
return s >= -BARY_EPS && w >= -BARY_EPS && s + w <= 1 + BARY_EPS;
|
|
63
|
+
}
|
|
64
|
+
|
|
65
|
+
/**
|
|
66
|
+
* Does the segment (ax,ay,az)->(bx,by,bz) pass THROUGH the mesh surface -- cross the interior of any
|
|
67
|
+
* triangle transversally (entering one side, exiting the other) strictly between its endpoints?
|
|
68
|
+
*
|
|
69
|
+
* This is the "penetration" test for a navmesh path piece. A piece that hugs the surface (coplanar with
|
|
70
|
+
* it) or flies over / under it -- a corner-only waypoint chord on a non-planar surface -- does NOT
|
|
71
|
+
* penetrate; a piece that tunnels through a wall or floor does. Endpoints lying on the surface (every
|
|
72
|
+
* path waypoint) are not penetrations.
|
|
73
|
+
*
|
|
74
|
+
* @param {BVH} bvh built by bvh_build_from_bt_mesh over `mesh`
|
|
75
|
+
* @param {BinaryTopology} mesh triangle mesh
|
|
76
|
+
* @param {number} ax
|
|
77
|
+
* @param {number} ay
|
|
78
|
+
* @param {number} az
|
|
79
|
+
* @param {number} bx
|
|
80
|
+
* @param {number} by
|
|
81
|
+
* @param {number} bz
|
|
82
|
+
* @returns {boolean}
|
|
83
|
+
*/
|
|
84
|
+
export function bvh_segment_penetrates_mesh(bvh, mesh, ax, ay, az, bx, by, bz) {
|
|
85
|
+
const root = bvh.root;
|
|
86
|
+
if (root === NULL_NODE) return false;
|
|
87
|
+
|
|
88
|
+
const float32 = bvh.__data_float32;
|
|
89
|
+
const uint32 = bvh.__data_uint32;
|
|
90
|
+
const dx = bx - ax, dy = by - ay, dz = bz - az;
|
|
91
|
+
|
|
92
|
+
let sp = 0;
|
|
93
|
+
_stack[sp++] = root;
|
|
94
|
+
|
|
95
|
+
while (sp > 0) {
|
|
96
|
+
const node = _stack[--sp];
|
|
97
|
+
const address = node * ELEMENT_WORD_COUNT;
|
|
98
|
+
|
|
99
|
+
if (!segment_intersects_aabb(
|
|
100
|
+
float32[address], float32[address + 1], float32[address + 2],
|
|
101
|
+
float32[address + 3], float32[address + 4], float32[address + 5],
|
|
102
|
+
ax, ay, az, dx, dy, dz
|
|
103
|
+
)) {
|
|
104
|
+
continue;
|
|
105
|
+
}
|
|
106
|
+
|
|
107
|
+
const child_1 = uint32[address + COLUMN_CHILD_1];
|
|
108
|
+
if (child_1 !== NULL_NODE) {
|
|
109
|
+
_stack[sp++] = child_1;
|
|
110
|
+
_stack[sp++] = uint32[address + COLUMN_CHILD_2];
|
|
111
|
+
continue;
|
|
112
|
+
}
|
|
113
|
+
|
|
114
|
+
const face_id = uint32[address + COLUMN_USER_DATA];
|
|
115
|
+
const loop_a = mesh.face_read_loop(face_id);
|
|
116
|
+
if (loop_a === NULL_POINTER) continue;
|
|
117
|
+
const loop_b = mesh.loop_read_next(loop_a);
|
|
118
|
+
const loop_c = mesh.loop_read_next(loop_b);
|
|
119
|
+
|
|
120
|
+
mesh.vertex_read_coordinate(_coords, 0, mesh.loop_read_vertex(loop_a));
|
|
121
|
+
const tax = _coords[0], tay = _coords[1], taz = _coords[2];
|
|
122
|
+
mesh.vertex_read_coordinate(_coords, 0, mesh.loop_read_vertex(loop_b));
|
|
123
|
+
const tbx = _coords[0], tby = _coords[1], tbz = _coords[2];
|
|
124
|
+
mesh.vertex_read_coordinate(_coords, 0, mesh.loop_read_vertex(loop_c));
|
|
125
|
+
const tcx = _coords[0], tcy = _coords[1], tcz = _coords[2];
|
|
126
|
+
|
|
127
|
+
if (segment_crosses_triangle(ax, ay, az, bx, by, bz, tax, tay, taz, tbx, tby, tbz, tcx, tcy, tcz)) {
|
|
128
|
+
return true;
|
|
129
|
+
}
|
|
130
|
+
}
|
|
131
|
+
|
|
132
|
+
return false;
|
|
133
|
+
}
|
|
@@ -1 +0,0 @@
|
|
|
1
|
-
{"version":3,"file":"navmesh_polyanya_find_path.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/navmesh_polyanya_find_path.js"],"names":[],"mappings":"AAqTA;;;;;;;;;;;;;;GAcG;AACH,mDAZW,MAAM,EAAE,GAAC,YAAY,GAAC,YAAY,gCAElC,MAAM,MACN,MAAM,MACN,MAAM,cACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,aACN,MAAM,GACJ,MAAM,CAmFlB"}
|