@inventure71/paddockjs 2.0.1 → 4.0.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/CHANGELOG.md +176 -1
- package/INSTALL_AND_UPDATE.md +24 -4
- package/README.md +84 -512
- package/docs/architecture.md +38 -24
- package/docs/component_inventory.md +72 -0
- package/docs/composable-layouts.md +77 -0
- package/docs/concepts.md +2 -2
- package/docs/custom_model_controller.md +6 -6
- package/docs/data-helpers.md +50 -0
- package/docs/data_contract.md +104 -27
- package/docs/getting-started.md +75 -0
- package/docs/headless-environment.md +68 -0
- package/docs/index.md +18 -4
- package/docs/learnings.md +6 -6
- package/docs/rules.md +12 -8
- package/docs/sense_contract.md +1 -1
- package/docs/system_specs.md +71 -21
- package/docs/theming.md +64 -0
- package/docs/training.md +26 -19
- package/docs/troubleshooting.md +76 -0
- package/docs/upgrading-hosts-to-3.md +42 -0
- package/examples/python/README.md +6 -0
- package/examples/python/base_policy_server.py +19 -0
- package/package.json +10 -3
- package/src/api/PaddockSimulatorController.js +150 -24
- package/src/api/runtimeTheme.js +59 -0
- package/src/app/F1SimulatorApp.js +213 -29
- package/src/app/banners/raceDataBannerController.js +71 -36
- package/src/app/banners/stewardMessageController.js +5 -4
- package/src/app/camera/cameraConstants.js +2 -0
- package/src/app/camera/cameraController.js +86 -29
- package/src/app/camera/cameraGeometry.js +12 -1
- package/src/app/domBindings.js +89 -9
- package/src/app/layoutScheduler.js +27 -0
- package/src/app/layoutSupport.js +165 -0
- package/src/app/raceOverlayClearanceSupport.js +158 -0
- package/src/app/readouts/carOverviewRenderer.js +16 -8
- package/src/app/readouts/controlStateRenderer.js +1 -1
- package/src/app/readouts/raceStatusRenderer.js +30 -21
- package/src/app/readouts/readoutFormatters.js +2 -0
- package/src/app/readouts/telemetryRenderer.js +17 -8
- package/src/app/readouts/timingTowerRenderer.js +262 -49
- package/src/app/runtime/frameLoop.js +5 -1
- package/src/app/runtime/runtimeVisibility.js +66 -5
- package/src/config/cssValues.js +16 -0
- package/src/config/defaultOptions.js +58 -38
- package/src/config/themeOptions.js +830 -0
- package/src/config/timingGapMode.js +13 -0
- package/src/config/urlOptions.js +25 -0
- package/src/data/championship.js +14 -9
- package/src/data/index.d.ts +212 -0
- package/src/data/index.js +14 -0
- package/src/environment/controllerLoop.js +17 -3
- package/src/environment/episode.js +11 -1
- package/src/environment/index.d.ts +1 -2
- package/src/environment/metrics.js +59 -16
- package/src/environment/observationVector.js +50 -33
- package/src/environment/observations.js +89 -20
- package/src/environment/options.js +1 -1
- package/src/environment/runtime.js +44 -11
- package/src/environment/scenarios.js +1 -1
- package/src/environment/sensors/carRays.js +48 -37
- package/src/environment/sensors/index.js +501 -97
- package/src/environment/sensors/indexedRayBands.js +134 -46
- package/src/environment/sensors/rayBandTrace.js +1171 -0
- package/src/environment/sensors/rayChannels.js +108 -0
- package/src/environment/sensors/rayConfig.js +71 -25
- package/src/environment/sensors/rayGeometry.js +23 -17
- package/src/environment/sensors/sensorTargets.js +67 -1
- package/src/environment/sensors/surfaceRays.js +167 -118
- package/src/environment/sensors/trackRays.js +157 -48
- package/src/index.d.ts +153 -9
- package/src/index.js +52 -3
- package/src/simulation/collisionGeometry.js +260 -96
- package/src/simulation/driver/driverController.js +1 -1
- package/src/simulation/driver/edgeRecovery.js +1 -1
- package/src/simulation/driver/racingControls.js +2 -2
- package/src/simulation/driver/rejoinControls.js +2 -2
- package/src/simulation/pit/pitFlow.js +21 -9
- package/src/simulation/pit/pitKinematics.js +2 -2
- package/src/simulation/pit/pitQueue.js +5 -3
- package/src/simulation/pit/pitRouteMovement.js +9 -4
- package/src/simulation/pit/pitRouting.js +79 -46
- package/src/simulation/pit/pitState.js +1 -0
- package/src/simulation/pit/pitTireService.js +7 -3
- package/src/simulation/race/raceFinish.js +25 -8
- package/src/simulation/race/raceOrder.js +189 -32
- package/src/simulation/race/raceProgress.js +129 -37
- package/src/simulation/race/raceSetup.js +4 -6
- package/src/simulation/race/raceStep.js +26 -14
- package/src/simulation/race/safetyCar.js +1 -1
- package/src/simulation/race/stalledDnf.js +1 -1
- package/src/simulation/raceSimulation.js +20 -17
- package/src/simulation/rules/collisionSteward.js +40 -23
- package/src/simulation/rules/rulesReview.js +77 -56
- package/src/simulation/snapshots/raceSnapshots.js +37 -0
- package/src/simulation/timing/drsTiming.js +143 -30
- package/src/simulation/timing/gapEstimation.js +25 -3
- package/src/simulation/timing/lapTelemetry.js +138 -40
- package/src/simulation/timing/raceTiming.js +5 -1
- package/src/simulation/timing/sectorPerformance.js +104 -26
- package/src/simulation/timing/timingHistory.js +229 -14
- package/src/simulation/timing/timingLines.js +65 -7
- package/src/simulation/track/pitLaneLayout.js +30 -3
- package/src/simulation/track/pitLaneState.js +218 -19
- package/src/simulation/track/spatialQueries.js +66 -33
- package/src/simulation/track/trackMath.js +91 -24
- package/src/simulation/track/trackModel.js +2 -0
- package/src/simulation/track/trackQueryIndex.js +1468 -189
- package/src/simulation/track/trackQueryPitIndex.js +138 -14
- package/src/simulation/track/trackQueryScratch.js +7 -0
- package/src/simulation/track/trackQueryStats.js +25 -0
- package/src/simulation/track/trackSnapshotJson.js +139 -0
- package/src/simulation/track/trackStatePolicy.js +249 -2
- package/src/simulation/vehicle/contactResolution.js +99 -62
- package/src/simulation/vehicle/mainTrackWheelSurface.js +73 -34
- package/src/simulation/vehicle/pitWheelSurface.js +81 -28
- package/src/simulation/vehicle/runoffResponse.js +54 -29
- package/src/simulation/vehicle/vehicleGeometry.js +245 -51
- package/src/simulation/vehicle/vehicleKinematics.js +2 -2
- package/src/simulation/vehicle/vehiclePhysics.js +45 -30
- package/src/simulation/vehicle/vehicleSnapshots.js +35 -1
- package/src/simulation/vehicle/vehicleState.js +7 -1
- package/src/simulation/vehicle/wheelSurface.js +807 -92
- package/src/simulation/warmup/runtimeWarmup.js +0 -1
- package/src/styles.css +1568 -519
- package/src/ui/bannerTemplates.js +38 -28
- package/src/ui/cameraControlsTemplate.js +16 -5
- package/src/ui/carOverviewTemplate.js +12 -5
- package/src/ui/raceCanvasTemplate.js +31 -8
- package/src/ui/raceControlsTemplate.js +27 -10
- package/src/ui/shellTemplate.js +3 -1
- package/src/ui/telemetryDrawerTemplate.js +23 -10
- package/src/ui/telemetryTemplates.js +58 -32
- package/src/ui/templateUtils.js +48 -0
- package/src/ui/timingTowerTemplate.js +29 -12
|
@@ -22,6 +22,9 @@ const GRID_NEIGHBOR_LIMIT = 2;
|
|
|
22
22
|
const ARC_BUCKET_COUNT = 512;
|
|
23
23
|
const AMBIGUOUS_DISTANCE_EPSILON = 1e-6;
|
|
24
24
|
const RAY_TRACE_BUCKET_DEGREES = 10;
|
|
25
|
+
const HINTED_RUNOFF_WIDE_SEGMENT_RADIUS = 20;
|
|
26
|
+
const PRECOMPUTED_SEGMENT_NEIGHBORHOOD_RADII = new Set([2, HINTED_RUNOFF_WIDE_SEGMENT_RADIUS]);
|
|
27
|
+
const LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD = 158;
|
|
25
28
|
|
|
26
29
|
export function createTrackQueryIndex(track) {
|
|
27
30
|
const samples = Array.isArray(track.samples) ? track.samples : [];
|
|
@@ -30,6 +33,7 @@ export function createTrackQueryIndex(track) {
|
|
|
30
33
|
const expansion = bands.runoffEdge + (track.barrierWidth ?? 0) + metersToSimUnits(64);
|
|
31
34
|
const bounds = expandedSampleBounds(samples, expansion);
|
|
32
35
|
const centerline = createCenterlineSegments(samples, segmentCount);
|
|
36
|
+
centerline.precomputedNeighborhoodsByRadius = createPrecomputedSegmentNeighborhoods(segmentCount, PRECOMPUTED_SEGMENT_NEIGHBORHOOD_RADII);
|
|
33
37
|
const grid = createSpatialGrid(bounds, DEFAULT_GRID_CELL_SIZE);
|
|
34
38
|
const segmentGrid = createSpatialGrid(bounds, DEFAULT_GRID_CELL_SIZE);
|
|
35
39
|
const arcBuckets = createArcBuckets(track.length, ARC_BUCKET_COUNT);
|
|
@@ -39,22 +43,34 @@ export function createTrackQueryIndex(track) {
|
|
|
39
43
|
insertSegmentIntoGrid(segmentGrid, centerline, segmentId, 0);
|
|
40
44
|
insertSegmentIntoArcBuckets(arcBuckets, centerline, segmentId);
|
|
41
45
|
}
|
|
46
|
+
finalizeArcBuckets(arcBuckets, segmentCount);
|
|
42
47
|
|
|
48
|
+
const lowDensityHintRing1Neighborhood = createLowDensityHintRing1Neighborhood(centerline, segmentGrid, LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD);
|
|
43
49
|
const index = {
|
|
44
50
|
version: 1,
|
|
45
51
|
bands,
|
|
52
|
+
nearestGridExpansion: expansion,
|
|
46
53
|
centerline,
|
|
47
54
|
grid,
|
|
48
55
|
segmentGrid,
|
|
49
56
|
arcBuckets,
|
|
57
|
+
nearestOccupiedCellRadiusByCell: createNearestOccupiedCellRadiusByCell(segmentGrid),
|
|
58
|
+
nearestNonLocalHintDistanceSquared: createNearestNonLocalHintDistanceSquared(centerline, segmentGrid, bands.kerbEdge * 2),
|
|
59
|
+
lowDensityHintRing1SegmentCounts: lowDensityHintRing1Neighborhood.counts,
|
|
60
|
+
lowDensityHintRing1CandidateIdsByCell: lowDensityHintRing1Neighborhood.candidateIdsByCell,
|
|
50
61
|
pit: createPitQueryIndex(track, grid.cellSize),
|
|
51
62
|
stats: createQueryStats(),
|
|
52
63
|
};
|
|
64
|
+
attachQueryStatsToCenterline(centerline, index.stats);
|
|
53
65
|
index.queryScratch = createQueryScratch(index);
|
|
54
66
|
return index;
|
|
55
67
|
}
|
|
56
68
|
|
|
57
|
-
export function queryNearestTrackProjection(track, position, progressHint = null
|
|
69
|
+
export function queryNearestTrackProjection(track, position, progressHint = null) {
|
|
70
|
+
return queryNearestTrackProjectionInto(track, position, progressHint);
|
|
71
|
+
}
|
|
72
|
+
|
|
73
|
+
export function queryNearestTrackProjectionInto(track, position, progressHint = null, target = null) {
|
|
58
74
|
const index = track?.queryIndex;
|
|
59
75
|
if (!index?.centerline?.segmentCount) {
|
|
60
76
|
recordFallback(index, 'nearestFallbackReasons', 'missing-index');
|
|
@@ -66,8 +82,7 @@ export function queryNearestTrackProjection(track, position, progressHint = null
|
|
|
66
82
|
}
|
|
67
83
|
index.stats.nearestQueries += 1;
|
|
68
84
|
|
|
69
|
-
const
|
|
70
|
-
const { projection, path, reason } = bestNearestFromIndex(track, index, position, progressHint, mode, options);
|
|
85
|
+
const { projection, path, reason } = bestProjectionFromIndex(index, position, progressHint, target);
|
|
71
86
|
if (!projection) {
|
|
72
87
|
recordFallback(index, 'nearestFallbackReasons', reason ?? 'unknown');
|
|
73
88
|
return null;
|
|
@@ -77,201 +92,896 @@ export function queryNearestTrackProjection(track, position, progressHint = null
|
|
|
77
92
|
return projection;
|
|
78
93
|
}
|
|
79
94
|
|
|
80
|
-
function
|
|
81
|
-
|
|
82
|
-
|
|
83
|
-
|
|
95
|
+
export function queryHintedTrackProjection(track, position, progressHint, { radius = 2 } = {}) {
|
|
96
|
+
const index = track?.queryIndex;
|
|
97
|
+
if (!index?.centerline?.segmentCount || !finitePoint(position) || !Number.isFinite(progressHint)) return null;
|
|
98
|
+
const local = bestProjectionFromHintedSegmentNeighborhood(
|
|
99
|
+
index,
|
|
100
|
+
progressHint,
|
|
101
|
+
Math.max(0, Math.floor(radius)),
|
|
102
|
+
position,
|
|
103
|
+
progressHint,
|
|
104
|
+
);
|
|
105
|
+
if (local.projection && local.projection.crossTrackError <= index.bands.gravelEdge) {
|
|
106
|
+
index.stats.hintedSegmentFastPathQueries += 1;
|
|
107
|
+
recordStat(index, 'hintedArcPaths', local.reason === 'tie-resolved' ? 'segment-hint-local-tie-resolved' : 'segment-hint-local');
|
|
108
|
+
return local.projection;
|
|
109
|
+
}
|
|
110
|
+
if (local.projection) {
|
|
111
|
+
index.stats.hintedSegmentWideRadiusQueries += 1;
|
|
112
|
+
const refined = bestProjectionFromHintedSegmentNeighborhood(
|
|
113
|
+
index,
|
|
114
|
+
progressHint,
|
|
115
|
+
HINTED_RUNOFF_WIDE_SEGMENT_RADIUS,
|
|
116
|
+
position,
|
|
117
|
+
progressHint,
|
|
118
|
+
);
|
|
119
|
+
if (refined.projection) {
|
|
120
|
+
if (!projectionMatches(local.projection, refined.projection)) {
|
|
121
|
+
index.stats.hintedSegmentWideRadiusHits += 1;
|
|
122
|
+
}
|
|
123
|
+
recordStat(
|
|
124
|
+
index,
|
|
125
|
+
'hintedArcPaths',
|
|
126
|
+
refined.reason === 'tie-resolved' ? 'segment-hint-runoff-wide-radius-tie-resolved' : 'segment-hint-runoff-wide-radius',
|
|
127
|
+
);
|
|
128
|
+
return refined.projection;
|
|
129
|
+
}
|
|
130
|
+
}
|
|
131
|
+
const { projection, reason } = bestProjectionFromArcBuckets(
|
|
132
|
+
index,
|
|
133
|
+
progressHint,
|
|
134
|
+
Math.max(0, Math.floor(radius)),
|
|
135
|
+
position,
|
|
136
|
+
progressHint,
|
|
137
|
+
);
|
|
138
|
+
if (!projection) return null;
|
|
139
|
+
index.stats.hintedArcQueries += 1;
|
|
140
|
+
recordStat(index, 'hintedArcPaths', reason === 'tie-resolved' ? 'arc-hint-local-tie-resolved' : 'arc-hint-local');
|
|
141
|
+
return projection;
|
|
142
|
+
}
|
|
143
|
+
|
|
144
|
+
export function querySegmentNeighborhoodProjection(
|
|
145
|
+
track,
|
|
146
|
+
position,
|
|
147
|
+
segmentId,
|
|
148
|
+
{ radius = 3, preferredDistance = null, target = null } = {},
|
|
149
|
+
) {
|
|
150
|
+
const index = track?.queryIndex;
|
|
151
|
+
if (!index?.centerline?.segmentCount || !finitePoint(position) || !Number.isInteger(segmentId)) return null;
|
|
152
|
+
const { projection, reason } = bestProjectionFromSegmentNeighborhood(
|
|
153
|
+
index.centerline,
|
|
154
|
+
segmentId,
|
|
155
|
+
Math.max(0, Math.floor(radius)),
|
|
156
|
+
position,
|
|
157
|
+
preferredDistance,
|
|
158
|
+
target,
|
|
159
|
+
);
|
|
160
|
+
if (!projection) return null;
|
|
161
|
+
index.stats.segmentNeighborhoodQueries += 1;
|
|
162
|
+
recordStat(
|
|
163
|
+
index,
|
|
164
|
+
'segmentNeighborhoodPaths',
|
|
165
|
+
reason === 'tie-resolved' ? 'segment-neighborhood-tie-resolved' : 'segment-neighborhood',
|
|
166
|
+
);
|
|
167
|
+
return projection;
|
|
168
|
+
}
|
|
169
|
+
|
|
170
|
+
export function querySegmentNeighborhoodProjections(
|
|
171
|
+
track,
|
|
172
|
+
positions,
|
|
173
|
+
segmentId,
|
|
174
|
+
{ radius = 3, preferredDistance = null, target = null } = {},
|
|
175
|
+
) {
|
|
176
|
+
const index = track?.queryIndex;
|
|
177
|
+
if (!index?.centerline?.segmentCount || !Array.isArray(positions) || !Number.isInteger(segmentId)) return null;
|
|
178
|
+
index.stats.segmentNeighborhoodBatchCalls += 1;
|
|
179
|
+
const projections = target ?? new Array(positions.length);
|
|
180
|
+
const tieResolvedFlags = bestProjectionBatchFromSegmentNeighborhood(
|
|
181
|
+
index,
|
|
182
|
+
positions,
|
|
183
|
+
segmentId,
|
|
184
|
+
Math.max(0, Math.floor(radius)),
|
|
185
|
+
preferredDistance,
|
|
186
|
+
projections,
|
|
187
|
+
);
|
|
188
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
189
|
+
const projection = projections[indexPosition];
|
|
190
|
+
if (!projection) continue;
|
|
191
|
+
index.stats.segmentNeighborhoodQueries += 1;
|
|
192
|
+
recordStat(
|
|
193
|
+
index,
|
|
194
|
+
'segmentNeighborhoodPaths',
|
|
195
|
+
tieResolvedFlags[indexPosition] ? 'segment-neighborhood-tie-resolved' : 'segment-neighborhood',
|
|
196
|
+
);
|
|
197
|
+
}
|
|
198
|
+
return projections;
|
|
84
199
|
}
|
|
85
200
|
|
|
86
|
-
function bestProjectionFromIndex(index, position, progressHint,
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
201
|
+
function bestProjectionFromIndex(index, position, progressHint, target = null) {
|
|
202
|
+
if (Number.isFinite(progressHint)) {
|
|
203
|
+
const isolatedHint = bestProjectionFromHintedSegmentNeighborhood(
|
|
204
|
+
index,
|
|
205
|
+
progressHint,
|
|
206
|
+
2,
|
|
207
|
+
position,
|
|
208
|
+
progressHint,
|
|
209
|
+
target,
|
|
210
|
+
);
|
|
211
|
+
if (
|
|
212
|
+
isolatedHint.projection &&
|
|
213
|
+
isolatedHint.projection.crossTrackError <= index.bands.kerbEdge &&
|
|
214
|
+
hintedQueryStaysInLocalCorridor(index, position, progressHint, index.bands.kerbEdge)
|
|
215
|
+
) {
|
|
216
|
+
if (isHintProjectionDistanceSafe(index, isolatedHint.projection)) {
|
|
217
|
+
index.stats.nearestIsolatedHintQueries += 1;
|
|
218
|
+
return {
|
|
219
|
+
projection: isolatedHint.projection,
|
|
220
|
+
path: isolatedHint.reason === 'tie-resolved' ? 'segment-hint-distance-gated-tie-resolved' : 'segment-hint-distance-gated',
|
|
221
|
+
};
|
|
222
|
+
}
|
|
223
|
+
if (isHintProjectionLowDensitySafe(index, position)) {
|
|
224
|
+
index.stats.nearestLowDensityHintQueries += 1;
|
|
225
|
+
return {
|
|
226
|
+
projection: isolatedHint.projection,
|
|
227
|
+
path: isolatedHint.reason === 'tie-resolved' ? 'segment-hint-density-gated-tie-resolved' : 'segment-hint-density-gated',
|
|
228
|
+
};
|
|
229
|
+
}
|
|
230
|
+
}
|
|
231
|
+
const lowDensityExact = bestProjectionFromLowDensityCellNeighborhood(index, position, progressHint, target);
|
|
232
|
+
if (lowDensityExact.projection) {
|
|
233
|
+
index.stats.nearestLowDensityCellExactQueries += 1;
|
|
234
|
+
index.stats.nearestLowDensityCellDirectQueries += 1;
|
|
94
235
|
return {
|
|
95
|
-
projection:
|
|
96
|
-
path:
|
|
236
|
+
projection: lowDensityExact.projection,
|
|
237
|
+
path: lowDensityExact.reason === 'tie-resolved'
|
|
238
|
+
? 'low-density-cell-exact-tie-resolved'
|
|
239
|
+
: 'low-density-cell-exact',
|
|
97
240
|
};
|
|
98
241
|
}
|
|
99
|
-
|
|
242
|
+
if (!hintedQueryStaysInLocalCorridor(index, position, progressHint, index.bands.runoffEdge)) {
|
|
243
|
+
const sparseExact = bestProjectionFromCellSearch(index, position, progressHint, null, target);
|
|
244
|
+
if (sparseExact.projection) {
|
|
245
|
+
index.stats.nearestSparseGridExactQueries += 1;
|
|
246
|
+
return {
|
|
247
|
+
projection: sparseExact.projection,
|
|
248
|
+
path: sparseExact.reason === 'tie-resolved'
|
|
249
|
+
? 'sparse-grid-exact-tie-resolved'
|
|
250
|
+
: 'sparse-grid-exact',
|
|
251
|
+
};
|
|
252
|
+
}
|
|
253
|
+
}
|
|
254
|
+
|
|
255
|
+
const hinted = bestProjectionFromArcBuckets(index, progressHint, 2, position, progressHint, target);
|
|
256
|
+
if (hinted.projection) {
|
|
257
|
+
let initialProjection = hinted.projection;
|
|
258
|
+
if (
|
|
259
|
+
hinted.projection.crossTrackError > index.bands.gravelEdge &&
|
|
260
|
+
hinted.projection.crossTrackError <= index.bands.runoffEdge + AMBIGUOUS_DISTANCE_EPSILON
|
|
261
|
+
) {
|
|
262
|
+
// Runoff-adjacent accurate-hint queries often converge inside the local radius-2 segment
|
|
263
|
+
// neighborhood; ring-3 cell lower bounds are enough to prove no outside segment can tie or beat it.
|
|
264
|
+
const ring2NeighborhoodExact = bestProjectionFromRing2Neighborhood(index, position, progressHint, target);
|
|
265
|
+
if (ring2NeighborhoodExact.projection) {
|
|
266
|
+
index.stats.nearestRing2NeighborhoodExactQueries += 1;
|
|
267
|
+
return {
|
|
268
|
+
projection: ring2NeighborhoodExact.projection,
|
|
269
|
+
path: ring2NeighborhoodExact.reason === 'tie-resolved'
|
|
270
|
+
? 'arc-hint-ring2-neighborhood-exact-tie-resolved'
|
|
271
|
+
: 'arc-hint-ring2-neighborhood-exact',
|
|
272
|
+
};
|
|
273
|
+
}
|
|
274
|
+
if (ring2NeighborhoodExact.bestProjection) {
|
|
275
|
+
initialProjection = ring2NeighborhoodExact.bestProjection;
|
|
276
|
+
}
|
|
277
|
+
}
|
|
278
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint, initialProjection);
|
|
279
|
+
if (exact.projection) {
|
|
280
|
+
const path = projectionMatches(hinted.projection, exact.projection)
|
|
281
|
+
? (hinted.reason === 'tie-resolved' ? 'arc-hint-tie-resolved' : 'arc-hint')
|
|
282
|
+
: 'arc-hint-refined';
|
|
283
|
+
return {
|
|
284
|
+
projection: exact.projection,
|
|
285
|
+
path: exact.reason === 'tie-resolved' && !path.endsWith('tie-resolved') ? `${path}-tie-resolved` : path,
|
|
286
|
+
};
|
|
287
|
+
}
|
|
288
|
+
}
|
|
100
289
|
}
|
|
101
290
|
|
|
291
|
+
let local = { projection: null, path: null };
|
|
102
292
|
const gridCandidateIds = candidateIdsFromGrid(index, index.grid, position, GRID_NEIGHBOR_LIMIT);
|
|
103
|
-
const grid = bestProjectionFromCandidates(index.centerline, gridCandidateIds, position, progressHint);
|
|
104
|
-
|
|
105
|
-
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
}
|
|
109
|
-
|
|
110
|
-
|
|
111
|
-
|
|
112
|
-
|
|
113
|
-
|
|
114
|
-
|
|
115
|
-
|
|
116
|
-
|
|
117
|
-
? candidateIdsFromArcBuckets(index, progressHint, 2)
|
|
118
|
-
: [];
|
|
119
|
-
if (hintedCandidateIds.length) {
|
|
120
|
-
const hinted = bestSampleFromCandidates(index, samples, sampleCount, hintedCandidateIds, position, progressHint);
|
|
121
|
-
const maxHintDistance = resolveHintMaxDistance(index, options);
|
|
122
|
-
if (hinted.sample && hinted.distanceSquared <= maxHintDistance * maxHintDistance) {
|
|
293
|
+
const grid = bestProjectionFromCandidates(index.centerline, gridCandidateIds, position, progressHint, target);
|
|
294
|
+
if (grid.projection) {
|
|
295
|
+
local = chooseBetterProjectionResult(local, grid, 'spatial-grid', progressHint);
|
|
296
|
+
if (grid.projection.distanceSquared <= index.nearestGridExpansion * index.nearestGridExpansion) {
|
|
297
|
+
return local;
|
|
298
|
+
}
|
|
299
|
+
}
|
|
300
|
+
|
|
301
|
+
if (local.projection) {
|
|
302
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint, local.projection);
|
|
303
|
+
if (exact.projection) {
|
|
304
|
+
const path = projectionMatches(local.projection, exact.projection)
|
|
305
|
+
? local.path
|
|
306
|
+
: `${local.path}-refined`;
|
|
123
307
|
return {
|
|
124
|
-
projection:
|
|
125
|
-
path:
|
|
308
|
+
projection: exact.projection,
|
|
309
|
+
path: exact.reason === 'tie-resolved' && !path.endsWith('tie-resolved') ? `${path}-tie-resolved` : path,
|
|
126
310
|
};
|
|
127
311
|
}
|
|
128
|
-
|
|
312
|
+
|
|
313
|
+
return {
|
|
314
|
+
projection: local.projection,
|
|
315
|
+
path: local.path,
|
|
316
|
+
};
|
|
129
317
|
}
|
|
130
318
|
|
|
131
|
-
const
|
|
132
|
-
const
|
|
133
|
-
return
|
|
319
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint);
|
|
320
|
+
const global = exact.projection ? exact : bestProjectionFromAllSegments(index.centerline, position, progressHint, target);
|
|
321
|
+
return global.projection
|
|
134
322
|
? {
|
|
135
|
-
projection:
|
|
136
|
-
path:
|
|
323
|
+
projection: global.projection,
|
|
324
|
+
path: exact.projection
|
|
325
|
+
? (global.reason === 'tie-resolved' ? 'exact-grid-tie-resolved' : 'exact-grid')
|
|
326
|
+
: (global.reason === 'tie-resolved' ? 'global-index-tie-resolved' : 'global-index'),
|
|
137
327
|
}
|
|
138
|
-
: { projection: null, reason: `
|
|
328
|
+
: { projection: null, reason: `global-index-${global.reason}` };
|
|
139
329
|
}
|
|
140
330
|
|
|
141
|
-
function
|
|
142
|
-
if (
|
|
143
|
-
|
|
144
|
-
return
|
|
331
|
+
function chooseBetterProjectionResult(current, candidate, source, preferredDistance) {
|
|
332
|
+
if (!candidate.projection) return current;
|
|
333
|
+
const path = candidate.reason === 'tie-resolved' ? `${source}-tie-resolved` : source;
|
|
334
|
+
if (!current.projection) return { projection: candidate.projection, path };
|
|
335
|
+
if (candidate.projection.distanceSquared < current.projection.distanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
336
|
+
return { projection: candidate.projection, path };
|
|
337
|
+
}
|
|
338
|
+
if (Math.abs(candidate.projection.distanceSquared - current.projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
339
|
+
const candidateTie = projectionTieScore(candidate.projection, preferredDistance);
|
|
340
|
+
const currentTie = projectionTieScore(current.projection, preferredDistance);
|
|
341
|
+
if (projectionTieBeats(candidate.projection, candidateTie, current.projection, currentTie)) {
|
|
342
|
+
return { projection: candidate.projection, path: `${path}-tie-resolved` };
|
|
343
|
+
}
|
|
344
|
+
}
|
|
345
|
+
return current;
|
|
145
346
|
}
|
|
146
347
|
|
|
147
|
-
function
|
|
148
|
-
|
|
348
|
+
function bestProjectionFromCellSearch(index, position, preferredDistance, initialProjection = null, target = null) {
|
|
349
|
+
const grid = index.segmentGrid ?? index.grid;
|
|
350
|
+
const center = gridCellForPoint(grid, position, true);
|
|
351
|
+
const startCellIndex = center.row * grid.columns + center.column;
|
|
352
|
+
const startRadius = index.nearestOccupiedCellRadiusByCell?.[startCellIndex] ?? 0;
|
|
353
|
+
if (startRadius > 0) {
|
|
354
|
+
index.stats.nearestEmptyCellSkippedRings += startRadius;
|
|
355
|
+
}
|
|
356
|
+
|
|
149
357
|
const scratch = ensureQueryScratch(index);
|
|
150
|
-
const
|
|
151
|
-
const
|
|
358
|
+
const segmentMarks = ensureScratchArray(scratch, 'exactSegmentMarks', index.centerline.segmentCount);
|
|
359
|
+
const cellMarks = ensureScratchArray(scratch, 'exactCellMarks', grid.columns * grid.rows);
|
|
360
|
+
const segmentEpoch = nextScratchEpoch(scratch, 'exactSegmentEpoch', segmentMarks);
|
|
361
|
+
const cellEpoch = nextScratchEpoch(scratch, 'exactCellEpoch', cellMarks);
|
|
362
|
+
const maxRadius = Math.max(grid.columns, grid.rows);
|
|
363
|
+
let bestSegmentId = initialProjection?.segmentId ?? null;
|
|
364
|
+
let bestX = initialProjection?.x ?? 0;
|
|
365
|
+
let bestY = initialProjection?.y ?? 0;
|
|
366
|
+
let bestDistance = initialProjection?.distance ?? 0;
|
|
367
|
+
let bestHeading = initialProjection?.heading ?? 0;
|
|
368
|
+
let bestNormalX = initialProjection?.normalX ?? 0;
|
|
369
|
+
let bestNormalY = initialProjection?.normalY ?? 0;
|
|
370
|
+
let bestCurvature = initialProjection?.curvature ?? 0;
|
|
371
|
+
let bestSignedOffset = initialProjection?.signedOffset ?? 0;
|
|
372
|
+
let bestDistanceSquared = initialProjection?.distanceSquared ?? Infinity;
|
|
373
|
+
let bestTieScore = initialProjection ? projectionTieScore(initialProjection, preferredDistance) : Infinity;
|
|
374
|
+
let secondBestDistance = Infinity;
|
|
375
|
+
let tieResolved = false;
|
|
376
|
+
if (Number.isInteger(bestSegmentId)) {
|
|
377
|
+
segmentMarks[bestSegmentId] = segmentEpoch;
|
|
378
|
+
}
|
|
379
|
+
|
|
380
|
+
for (let radius = startRadius; radius <= maxRadius; radius += 1) {
|
|
381
|
+
let ringCanImprove = bestSegmentId == null;
|
|
382
|
+
forEachRingCell(grid, center, radius, (cellIndex, row, column) => {
|
|
383
|
+
if (cellMarks[cellIndex] === cellEpoch) return;
|
|
384
|
+
cellMarks[cellIndex] = cellEpoch;
|
|
385
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
386
|
+
if (bestSegmentId != null && lowerBound > bestDistanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
387
|
+
return;
|
|
388
|
+
}
|
|
389
|
+
ringCanImprove = true;
|
|
390
|
+
const cell = grid.cells[cellIndex];
|
|
391
|
+
if (!cell) return;
|
|
392
|
+
for (const segmentId of cell) {
|
|
393
|
+
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
394
|
+
segmentMarks[segmentId] = segmentEpoch;
|
|
395
|
+
const ax = index.centerline.startX[segmentId];
|
|
396
|
+
const ay = index.centerline.startY[segmentId];
|
|
397
|
+
const dx = index.centerline.deltaX[segmentId];
|
|
398
|
+
const dy = index.centerline.deltaY[segmentId];
|
|
399
|
+
const amount = index.centerline.lengthSquared[segmentId] > 0
|
|
400
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / index.centerline.lengthSquared[segmentId], 0, 1)
|
|
401
|
+
: 0;
|
|
402
|
+
const x = ax + dx * amount;
|
|
403
|
+
const y = ay + dy * amount;
|
|
404
|
+
const distance = index.centerline.startDistance[segmentId] +
|
|
405
|
+
index.centerline.distanceSpan[segmentId] * amount;
|
|
406
|
+
const normalX = index.centerline.normalX[segmentId];
|
|
407
|
+
const normalY = index.centerline.normalY[segmentId];
|
|
408
|
+
const px = position.x - x;
|
|
409
|
+
const py = position.y - y;
|
|
410
|
+
const signedOffset = px * normalX + py * normalY;
|
|
411
|
+
const distanceSquared = px * px + py * py;
|
|
412
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
413
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
414
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
415
|
+
bestSegmentId = segmentId;
|
|
416
|
+
bestX = x;
|
|
417
|
+
bestY = y;
|
|
418
|
+
bestDistance = distance;
|
|
419
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
420
|
+
bestNormalX = normalX;
|
|
421
|
+
bestNormalY = normalY;
|
|
422
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
423
|
+
bestSignedOffset = signedOffset;
|
|
424
|
+
bestDistanceSquared = distanceSquared;
|
|
425
|
+
bestTieScore = tieScore;
|
|
426
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
427
|
+
tieResolved = true;
|
|
428
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
429
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
430
|
+
bestSegmentId = segmentId;
|
|
431
|
+
bestX = x;
|
|
432
|
+
bestY = y;
|
|
433
|
+
bestDistance = distance;
|
|
434
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
435
|
+
bestNormalX = normalX;
|
|
436
|
+
bestNormalY = normalY;
|
|
437
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
438
|
+
bestSignedOffset = signedOffset;
|
|
439
|
+
bestDistanceSquared = distanceSquared;
|
|
440
|
+
bestTieScore = tieScore;
|
|
441
|
+
}
|
|
442
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
443
|
+
secondBestDistance = distanceSquared;
|
|
444
|
+
}
|
|
445
|
+
}
|
|
446
|
+
});
|
|
447
|
+
if (bestSegmentId != null && !ringCanImprove) break;
|
|
448
|
+
}
|
|
449
|
+
|
|
450
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-candidates' };
|
|
451
|
+
const projection = initialProjection ?? target ?? {};
|
|
452
|
+
writeProjectionCandidate(projection, {
|
|
453
|
+
segmentId: bestSegmentId,
|
|
454
|
+
x: bestX,
|
|
455
|
+
y: bestY,
|
|
456
|
+
distance: bestDistance,
|
|
457
|
+
heading: bestHeading,
|
|
458
|
+
normalX: bestNormalX,
|
|
459
|
+
normalY: bestNormalY,
|
|
460
|
+
curvature: bestCurvature,
|
|
461
|
+
signedOffset: bestSignedOffset,
|
|
462
|
+
distanceSquared: bestDistanceSquared,
|
|
463
|
+
});
|
|
464
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
465
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
466
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
467
|
+
}
|
|
468
|
+
|
|
469
|
+
function projectionMatches(first, second) {
|
|
470
|
+
return first.segmentId === second.segmentId &&
|
|
471
|
+
Math.abs(first.distanceSquared - second.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
472
|
+
}
|
|
473
|
+
|
|
474
|
+
function forEachRingCell(grid, center, radius, visitor) {
|
|
475
|
+
const minRow = Math.max(0, center.row - radius);
|
|
476
|
+
const maxRow = Math.min(grid.rows - 1, center.row + radius);
|
|
477
|
+
const minColumn = Math.max(0, center.column - radius);
|
|
478
|
+
const maxColumn = Math.min(grid.columns - 1, center.column + radius);
|
|
479
|
+
for (let row = minRow; row <= maxRow; row += 1) {
|
|
480
|
+
for (let column = minColumn; column <= maxColumn; column += 1) {
|
|
481
|
+
if (
|
|
482
|
+
radius > 0 &&
|
|
483
|
+
row !== minRow &&
|
|
484
|
+
row !== maxRow &&
|
|
485
|
+
column !== minColumn &&
|
|
486
|
+
column !== maxColumn
|
|
487
|
+
) continue;
|
|
488
|
+
visitor(row * grid.columns + column, row, column);
|
|
489
|
+
}
|
|
490
|
+
}
|
|
491
|
+
}
|
|
492
|
+
|
|
493
|
+
function cellDistanceSquared(grid, row, column, position) {
|
|
494
|
+
const minX = grid.bounds.minX + column * grid.cellSize;
|
|
495
|
+
const minY = grid.bounds.minY + row * grid.cellSize;
|
|
496
|
+
const maxX = Math.min(grid.bounds.maxX, minX + grid.cellSize);
|
|
497
|
+
const maxY = Math.min(grid.bounds.maxY, minY + grid.cellSize);
|
|
498
|
+
const dx = position.x < minX ? minX - position.x : position.x > maxX ? position.x - maxX : 0;
|
|
499
|
+
const dy = position.y < minY ? minY - position.y : position.y > maxY ? position.y - maxY : 0;
|
|
500
|
+
return dx * dx + dy * dy;
|
|
501
|
+
}
|
|
152
502
|
|
|
153
|
-
|
|
503
|
+
function bestProjectionFromCandidates(centerline, candidateIds, position, preferredDistance = null, target = null) {
|
|
504
|
+
if (!candidateIds.length) return { projection: null, reason: 'no-candidates' };
|
|
505
|
+
let bestSegmentId = null;
|
|
506
|
+
let bestX = 0;
|
|
507
|
+
let bestY = 0;
|
|
508
|
+
let bestDistance = 0;
|
|
509
|
+
let bestHeading = 0;
|
|
510
|
+
let bestNormalX = 0;
|
|
511
|
+
let bestNormalY = 0;
|
|
512
|
+
let bestCurvature = 0;
|
|
513
|
+
let bestSignedOffset = 0;
|
|
154
514
|
let bestDistanceSquared = Infinity;
|
|
155
515
|
let bestTieScore = Infinity;
|
|
156
516
|
let secondBestDistance = Infinity;
|
|
157
517
|
let tieResolved = false;
|
|
158
|
-
for (const segmentId of
|
|
159
|
-
const
|
|
160
|
-
const
|
|
161
|
-
|
|
162
|
-
|
|
163
|
-
|
|
164
|
-
|
|
165
|
-
|
|
166
|
-
|
|
167
|
-
|
|
168
|
-
|
|
169
|
-
|
|
170
|
-
|
|
171
|
-
|
|
172
|
-
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
|
|
176
|
-
|
|
177
|
-
|
|
178
|
-
|
|
179
|
-
|
|
180
|
-
|
|
181
|
-
|
|
182
|
-
|
|
183
|
-
|
|
184
|
-
|
|
185
|
-
|
|
186
|
-
|
|
187
|
-
|
|
188
|
-
|
|
189
|
-
|
|
190
|
-
|
|
191
|
-
|
|
518
|
+
for (const segmentId of candidateIds) {
|
|
519
|
+
const ax = centerline.startX[segmentId];
|
|
520
|
+
const ay = centerline.startY[segmentId];
|
|
521
|
+
const dx = centerline.deltaX[segmentId];
|
|
522
|
+
const dy = centerline.deltaY[segmentId];
|
|
523
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
524
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
525
|
+
: 0;
|
|
526
|
+
const x = ax + dx * amount;
|
|
527
|
+
const y = ay + dy * amount;
|
|
528
|
+
const distance = centerline.startDistance[segmentId] +
|
|
529
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
530
|
+
const normalX = centerline.normalX[segmentId];
|
|
531
|
+
const normalY = centerline.normalY[segmentId];
|
|
532
|
+
const px = position.x - x;
|
|
533
|
+
const py = position.y - y;
|
|
534
|
+
const signedOffset = px * normalX + py * normalY;
|
|
535
|
+
const distanceSquared = px * px + py * py;
|
|
536
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
537
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
538
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
539
|
+
bestSegmentId = segmentId;
|
|
540
|
+
bestX = x;
|
|
541
|
+
bestY = y;
|
|
542
|
+
bestDistance = distance;
|
|
543
|
+
bestHeading = centerline.heading[segmentId];
|
|
544
|
+
bestNormalX = normalX;
|
|
545
|
+
bestNormalY = normalY;
|
|
546
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
547
|
+
bestSignedOffset = signedOffset;
|
|
548
|
+
bestDistanceSquared = distanceSquared;
|
|
549
|
+
bestTieScore = tieScore;
|
|
550
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
551
|
+
tieResolved = true;
|
|
552
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
553
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
554
|
+
bestSegmentId = segmentId;
|
|
555
|
+
bestX = x;
|
|
556
|
+
bestY = y;
|
|
557
|
+
bestDistance = distance;
|
|
558
|
+
bestHeading = centerline.heading[segmentId];
|
|
559
|
+
bestNormalX = normalX;
|
|
560
|
+
bestNormalY = normalY;
|
|
561
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
562
|
+
bestSignedOffset = signedOffset;
|
|
563
|
+
bestDistanceSquared = distanceSquared;
|
|
564
|
+
bestTieScore = tieScore;
|
|
565
|
+
}
|
|
566
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
567
|
+
secondBestDistance = distanceSquared;
|
|
568
|
+
}
|
|
569
|
+
}
|
|
570
|
+
|
|
571
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
572
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
573
|
+
segmentId: bestSegmentId,
|
|
574
|
+
x: bestX,
|
|
575
|
+
y: bestY,
|
|
576
|
+
distance: bestDistance,
|
|
577
|
+
heading: bestHeading,
|
|
578
|
+
normalX: bestNormalX,
|
|
579
|
+
normalY: bestNormalY,
|
|
580
|
+
curvature: bestCurvature,
|
|
581
|
+
signedOffset: bestSignedOffset,
|
|
582
|
+
distanceSquared: bestDistanceSquared,
|
|
583
|
+
});
|
|
192
584
|
|
|
193
585
|
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
194
586
|
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
195
|
-
return {
|
|
196
|
-
sample: bestSample,
|
|
197
|
-
distanceSquared: bestDistanceSquared,
|
|
198
|
-
reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok',
|
|
199
|
-
};
|
|
587
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
200
588
|
}
|
|
201
589
|
|
|
202
|
-
function
|
|
203
|
-
|
|
204
|
-
|
|
205
|
-
|
|
206
|
-
|
|
207
|
-
|
|
208
|
-
|
|
209
|
-
|
|
210
|
-
|
|
211
|
-
|
|
212
|
-
|
|
213
|
-
|
|
214
|
-
|
|
215
|
-
|
|
216
|
-
|
|
217
|
-
|
|
218
|
-
|
|
219
|
-
|
|
220
|
-
|
|
221
|
-
|
|
222
|
-
|
|
223
|
-
|
|
224
|
-
|
|
225
|
-
|
|
226
|
-
|
|
227
|
-
|
|
228
|
-
|
|
229
|
-
|
|
230
|
-
|
|
590
|
+
function bestProjectionFromAllSegments(centerline, position, preferredDistance = null, target = null) {
|
|
591
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
592
|
+
if (segmentCount <= 0) return { projection: null, reason: 'no-candidates' };
|
|
593
|
+
let bestSegmentId = null;
|
|
594
|
+
let bestX = 0;
|
|
595
|
+
let bestY = 0;
|
|
596
|
+
let bestDistance = 0;
|
|
597
|
+
let bestHeading = 0;
|
|
598
|
+
let bestNormalX = 0;
|
|
599
|
+
let bestNormalY = 0;
|
|
600
|
+
let bestCurvature = 0;
|
|
601
|
+
let bestSignedOffset = 0;
|
|
602
|
+
let bestDistanceSquared = Infinity;
|
|
603
|
+
let bestTieScore = Infinity;
|
|
604
|
+
let secondBestDistance = Infinity;
|
|
605
|
+
let tieResolved = false;
|
|
606
|
+
|
|
607
|
+
for (let segmentId = 0; segmentId < segmentCount; segmentId += 1) {
|
|
608
|
+
const ax = centerline.startX[segmentId];
|
|
609
|
+
const ay = centerline.startY[segmentId];
|
|
610
|
+
const dx = centerline.deltaX[segmentId];
|
|
611
|
+
const dy = centerline.deltaY[segmentId];
|
|
612
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
613
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
614
|
+
: 0;
|
|
615
|
+
const x = ax + dx * amount;
|
|
616
|
+
const y = ay + dy * amount;
|
|
617
|
+
const distance = centerline.startDistance[segmentId] +
|
|
618
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
619
|
+
const normalX = centerline.normalX[segmentId];
|
|
620
|
+
const normalY = centerline.normalY[segmentId];
|
|
621
|
+
const px = position.x - x;
|
|
622
|
+
const py = position.y - y;
|
|
623
|
+
const signedOffset = px * normalX + py * normalY;
|
|
624
|
+
const distanceSquared = px * px + py * py;
|
|
625
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
626
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
627
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
628
|
+
bestSegmentId = segmentId;
|
|
629
|
+
bestX = x;
|
|
630
|
+
bestY = y;
|
|
631
|
+
bestDistance = distance;
|
|
632
|
+
bestHeading = centerline.heading[segmentId];
|
|
633
|
+
bestNormalX = normalX;
|
|
634
|
+
bestNormalY = normalY;
|
|
635
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
636
|
+
bestSignedOffset = signedOffset;
|
|
231
637
|
bestDistanceSquared = distanceSquared;
|
|
232
638
|
bestTieScore = tieScore;
|
|
639
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
640
|
+
tieResolved = true;
|
|
641
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
642
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
643
|
+
bestSegmentId = segmentId;
|
|
644
|
+
bestX = x;
|
|
645
|
+
bestY = y;
|
|
646
|
+
bestDistance = distance;
|
|
647
|
+
bestHeading = centerline.heading[segmentId];
|
|
648
|
+
bestNormalX = normalX;
|
|
649
|
+
bestNormalY = normalY;
|
|
650
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
651
|
+
bestSignedOffset = signedOffset;
|
|
652
|
+
bestDistanceSquared = distanceSquared;
|
|
653
|
+
bestTieScore = tieScore;
|
|
654
|
+
}
|
|
655
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
656
|
+
secondBestDistance = distanceSquared;
|
|
233
657
|
}
|
|
234
|
-
} else if (distanceSquared < secondBestDistance) {
|
|
235
|
-
secondBestDistance = distanceSquared;
|
|
236
658
|
}
|
|
237
|
-
return { bestSample, bestDistanceSquared, bestTieScore, secondBestDistance, tieResolved };
|
|
238
|
-
}
|
|
239
659
|
|
|
240
|
-
|
|
241
|
-
|
|
242
|
-
|
|
660
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
661
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
662
|
+
segmentId: bestSegmentId,
|
|
663
|
+
x: bestX,
|
|
664
|
+
y: bestY,
|
|
665
|
+
distance: bestDistance,
|
|
666
|
+
heading: bestHeading,
|
|
667
|
+
normalX: bestNormalX,
|
|
668
|
+
normalY: bestNormalY,
|
|
669
|
+
curvature: bestCurvature,
|
|
670
|
+
signedOffset: bestSignedOffset,
|
|
671
|
+
distanceSquared: bestDistanceSquared,
|
|
672
|
+
});
|
|
673
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
674
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
675
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
243
676
|
}
|
|
244
677
|
|
|
245
|
-
function
|
|
246
|
-
|
|
247
|
-
|
|
678
|
+
function bestProjectionFromSegmentNeighborhood(
|
|
679
|
+
centerline,
|
|
680
|
+
centerSegmentId,
|
|
681
|
+
radius,
|
|
682
|
+
position,
|
|
683
|
+
preferredDistance = null,
|
|
684
|
+
target = null,
|
|
685
|
+
) {
|
|
686
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
687
|
+
if (segmentCount <= 0) return { projection: null, reason: 'no-candidates' };
|
|
688
|
+
let bestSegmentId = null;
|
|
689
|
+
let bestX = 0;
|
|
690
|
+
let bestY = 0;
|
|
691
|
+
let bestDistance = 0;
|
|
692
|
+
let bestHeading = 0;
|
|
693
|
+
let bestNormalX = 0;
|
|
694
|
+
let bestNormalY = 0;
|
|
695
|
+
let bestCurvature = 0;
|
|
696
|
+
let bestSignedOffset = 0;
|
|
697
|
+
let bestDistanceSquared = Infinity;
|
|
248
698
|
let bestTieScore = Infinity;
|
|
249
699
|
let secondBestDistance = Infinity;
|
|
250
700
|
let tieResolved = false;
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
254
|
-
|
|
255
|
-
|
|
256
|
-
|
|
701
|
+
|
|
702
|
+
const precomputedNeighborhood = getPrecomputedSegmentNeighborhood(centerline, centerSegmentId, radius);
|
|
703
|
+
const segmentIds = precomputedNeighborhood ?? null;
|
|
704
|
+
if (centerline?.stats && segmentIds) centerline.stats.precomputedSegmentNeighborhoodHits += 1;
|
|
705
|
+
const segmentIdCount = segmentIds?.length ?? (radius * 2 + 1);
|
|
706
|
+
for (let index = 0; index < segmentIdCount; index += 1) {
|
|
707
|
+
const segmentId = segmentIds
|
|
708
|
+
? segmentIds[index]
|
|
709
|
+
: ((centerSegmentId + (index - radius)) % segmentCount + segmentCount) % segmentCount;
|
|
710
|
+
const ax = centerline.startX[segmentId];
|
|
711
|
+
const ay = centerline.startY[segmentId];
|
|
712
|
+
const dx = centerline.deltaX[segmentId];
|
|
713
|
+
const dy = centerline.deltaY[segmentId];
|
|
714
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
715
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
716
|
+
: 0;
|
|
717
|
+
const x = ax + dx * amount;
|
|
718
|
+
const y = ay + dy * amount;
|
|
719
|
+
const distance = centerline.startDistance[segmentId] +
|
|
720
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
721
|
+
const normalX = centerline.normalX[segmentId];
|
|
722
|
+
const normalY = centerline.normalY[segmentId];
|
|
723
|
+
const px = position.x - x;
|
|
724
|
+
const py = position.y - y;
|
|
725
|
+
const signedOffset = px * normalX + py * normalY;
|
|
726
|
+
const distanceSquared = px * px + py * py;
|
|
727
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
728
|
+
|
|
729
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
730
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
731
|
+
bestSegmentId = segmentId;
|
|
732
|
+
bestX = x;
|
|
733
|
+
bestY = y;
|
|
734
|
+
bestDistance = distance;
|
|
735
|
+
bestHeading = centerline.heading[segmentId];
|
|
736
|
+
bestNormalX = normalX;
|
|
737
|
+
bestNormalY = normalY;
|
|
738
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
739
|
+
bestSignedOffset = signedOffset;
|
|
740
|
+
bestDistanceSquared = distanceSquared;
|
|
257
741
|
bestTieScore = tieScore;
|
|
258
|
-
|
|
742
|
+
continue;
|
|
743
|
+
}
|
|
744
|
+
|
|
745
|
+
if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
259
746
|
tieResolved = true;
|
|
260
|
-
secondBestDistance = Math.min(secondBestDistance,
|
|
261
|
-
if (tieScore
|
|
262
|
-
|
|
747
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
748
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
749
|
+
bestSegmentId = segmentId;
|
|
750
|
+
bestX = x;
|
|
751
|
+
bestY = y;
|
|
752
|
+
bestDistance = distance;
|
|
753
|
+
bestHeading = centerline.heading[segmentId];
|
|
754
|
+
bestNormalX = normalX;
|
|
755
|
+
bestNormalY = normalY;
|
|
756
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
757
|
+
bestSignedOffset = signedOffset;
|
|
758
|
+
bestDistanceSquared = distanceSquared;
|
|
263
759
|
bestTieScore = tieScore;
|
|
264
760
|
}
|
|
265
|
-
|
|
266
|
-
secondBestDistance = projection.distanceSquared;
|
|
761
|
+
continue;
|
|
267
762
|
}
|
|
268
|
-
}
|
|
269
763
|
|
|
270
|
-
|
|
764
|
+
if (distanceSquared < secondBestDistance) {
|
|
765
|
+
secondBestDistance = distanceSquared;
|
|
766
|
+
}
|
|
767
|
+
}
|
|
271
768
|
|
|
769
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
770
|
+
const projection = target ?? {};
|
|
771
|
+
projection.segmentId = bestSegmentId;
|
|
772
|
+
projection.x = bestX;
|
|
773
|
+
projection.y = bestY;
|
|
774
|
+
projection.distance = bestDistance;
|
|
775
|
+
projection.heading = bestHeading;
|
|
776
|
+
projection.normalX = bestNormalX;
|
|
777
|
+
projection.normalY = bestNormalY;
|
|
778
|
+
projection.curvature = bestCurvature;
|
|
779
|
+
projection.signedOffset = bestSignedOffset;
|
|
780
|
+
projection.crossTrackError = Math.abs(bestSignedOffset);
|
|
781
|
+
projection.distanceSquared = bestDistanceSquared;
|
|
272
782
|
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
273
|
-
Math.abs(secondBestDistance -
|
|
274
|
-
return { projection
|
|
783
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
784
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
785
|
+
}
|
|
786
|
+
|
|
787
|
+
function bestProjectionBatchFromSegmentNeighborhood(
|
|
788
|
+
index,
|
|
789
|
+
positions,
|
|
790
|
+
centerSegmentId,
|
|
791
|
+
radius,
|
|
792
|
+
preferredDistance = null,
|
|
793
|
+
target = null,
|
|
794
|
+
) {
|
|
795
|
+
const centerline = index.centerline;
|
|
796
|
+
const projections = target ?? new Array(positions.length);
|
|
797
|
+
projections.length = positions.length;
|
|
798
|
+
const scratch = ensureQueryScratch(index);
|
|
799
|
+
const tieResolvedFlags = ensureUint8ScratchArray(scratch, 'segmentNeighborhoodTieResolved', positions.length);
|
|
800
|
+
const validFlags = ensureUint8ScratchArray(scratch, 'segmentNeighborhoodValid', positions.length);
|
|
801
|
+
const bestTieScores = ensureFloat64ScratchArray(scratch, 'segmentNeighborhoodBestTieScores', positions.length);
|
|
802
|
+
const secondBestDistances = ensureFloat64ScratchArray(scratch, 'segmentNeighborhoodSecondBestDistances', positions.length);
|
|
803
|
+
|
|
804
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
805
|
+
const position = positions[indexPosition];
|
|
806
|
+
if (!finitePoint(position)) {
|
|
807
|
+
projections[indexPosition] = null;
|
|
808
|
+
validFlags[indexPosition] = 0;
|
|
809
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
810
|
+
bestTieScores[indexPosition] = Infinity;
|
|
811
|
+
secondBestDistances[indexPosition] = Infinity;
|
|
812
|
+
continue;
|
|
813
|
+
}
|
|
814
|
+
const projection = projections[indexPosition] ?? {};
|
|
815
|
+
projections[indexPosition] = projection;
|
|
816
|
+
projection.segmentId = -1;
|
|
817
|
+
projection.distanceSquared = Infinity;
|
|
818
|
+
validFlags[indexPosition] = 1;
|
|
819
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
820
|
+
bestTieScores[indexPosition] = Infinity;
|
|
821
|
+
secondBestDistances[indexPosition] = Infinity;
|
|
822
|
+
}
|
|
823
|
+
|
|
824
|
+
const segmentCount = centerline.segmentCount;
|
|
825
|
+
const precomputedNeighborhood = getPrecomputedSegmentNeighborhood(centerline, centerSegmentId, radius);
|
|
826
|
+
if (centerline?.stats && precomputedNeighborhood) centerline.stats.precomputedSegmentNeighborhoodHits += positions.length;
|
|
827
|
+
const segmentIds = precomputedNeighborhood ?? null;
|
|
828
|
+
const segmentIdCount = segmentIds?.length ?? (radius * 2 + 1);
|
|
829
|
+
for (let index = 0; index < segmentIdCount; index += 1) {
|
|
830
|
+
const segmentId = segmentIds
|
|
831
|
+
? segmentIds[index]
|
|
832
|
+
: ((centerSegmentId + (index - radius)) % segmentCount + segmentCount) % segmentCount;
|
|
833
|
+
const ax = centerline.startX[segmentId];
|
|
834
|
+
const ay = centerline.startY[segmentId];
|
|
835
|
+
const dx = centerline.deltaX[segmentId];
|
|
836
|
+
const dy = centerline.deltaY[segmentId];
|
|
837
|
+
const lengthSquared = centerline.lengthSquared[segmentId];
|
|
838
|
+
const distanceBase = centerline.startDistance[segmentId];
|
|
839
|
+
const distanceSpan = centerline.distanceSpan[segmentId];
|
|
840
|
+
const heading = centerline.heading[segmentId];
|
|
841
|
+
const normalX = centerline.normalX[segmentId];
|
|
842
|
+
const normalY = centerline.normalY[segmentId];
|
|
843
|
+
const curvature = centerline.curvature[segmentId];
|
|
844
|
+
|
|
845
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
846
|
+
if (!validFlags[indexPosition]) continue;
|
|
847
|
+
const position = positions[indexPosition];
|
|
848
|
+
const amount = lengthSquared > 0
|
|
849
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / lengthSquared, 0, 1)
|
|
850
|
+
: 0;
|
|
851
|
+
const x = ax + dx * amount;
|
|
852
|
+
const y = ay + dy * amount;
|
|
853
|
+
const distance = distanceBase + distanceSpan * amount;
|
|
854
|
+
const px = position.x - x;
|
|
855
|
+
const py = position.y - y;
|
|
856
|
+
const signedOffset = px * normalX + py * normalY;
|
|
857
|
+
const distanceSquared = px * px + py * py;
|
|
858
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
859
|
+
const projection = projections[indexPosition];
|
|
860
|
+
|
|
861
|
+
if (projection.segmentId < 0 || distanceSquared < projection.distanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
862
|
+
secondBestDistances[indexPosition] = projection.segmentId < 0 ? Infinity : projection.distanceSquared;
|
|
863
|
+
writeProjectionCandidate(projection, {
|
|
864
|
+
segmentId,
|
|
865
|
+
x,
|
|
866
|
+
y,
|
|
867
|
+
distance,
|
|
868
|
+
heading,
|
|
869
|
+
normalX,
|
|
870
|
+
normalY,
|
|
871
|
+
curvature,
|
|
872
|
+
signedOffset,
|
|
873
|
+
distanceSquared,
|
|
874
|
+
});
|
|
875
|
+
bestTieScores[indexPosition] = tieScore;
|
|
876
|
+
continue;
|
|
877
|
+
}
|
|
878
|
+
|
|
879
|
+
if (Math.abs(distanceSquared - projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
880
|
+
tieResolvedFlags[indexPosition] = 1;
|
|
881
|
+
secondBestDistances[indexPosition] = Math.min(secondBestDistances[indexPosition], distanceSquared);
|
|
882
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, projection.segmentId, bestTieScores[indexPosition])) {
|
|
883
|
+
writeProjectionCandidate(projection, {
|
|
884
|
+
segmentId,
|
|
885
|
+
x,
|
|
886
|
+
y,
|
|
887
|
+
distance,
|
|
888
|
+
heading,
|
|
889
|
+
normalX,
|
|
890
|
+
normalY,
|
|
891
|
+
curvature,
|
|
892
|
+
signedOffset,
|
|
893
|
+
distanceSquared,
|
|
894
|
+
});
|
|
895
|
+
bestTieScores[indexPosition] = tieScore;
|
|
896
|
+
}
|
|
897
|
+
continue;
|
|
898
|
+
}
|
|
899
|
+
|
|
900
|
+
if (distanceSquared < secondBestDistances[indexPosition]) {
|
|
901
|
+
secondBestDistances[indexPosition] = distanceSquared;
|
|
902
|
+
}
|
|
903
|
+
}
|
|
904
|
+
}
|
|
905
|
+
|
|
906
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
907
|
+
if (!validFlags[indexPosition]) continue;
|
|
908
|
+
const projection = projections[indexPosition];
|
|
909
|
+
if (projection.segmentId < 0) {
|
|
910
|
+
projections[indexPosition] = null;
|
|
911
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
912
|
+
continue;
|
|
913
|
+
}
|
|
914
|
+
const ambiguous = Number.isFinite(secondBestDistances[indexPosition]) &&
|
|
915
|
+
Math.abs(secondBestDistances[indexPosition] - projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
916
|
+
if (ambiguous) tieResolvedFlags[indexPosition] = 1;
|
|
917
|
+
}
|
|
918
|
+
|
|
919
|
+
return tieResolvedFlags;
|
|
920
|
+
}
|
|
921
|
+
|
|
922
|
+
function writeProjectionCandidate(target, {
|
|
923
|
+
segmentId,
|
|
924
|
+
x,
|
|
925
|
+
y,
|
|
926
|
+
distance,
|
|
927
|
+
heading,
|
|
928
|
+
normalX,
|
|
929
|
+
normalY,
|
|
930
|
+
curvature,
|
|
931
|
+
signedOffset,
|
|
932
|
+
distanceSquared,
|
|
933
|
+
}) {
|
|
934
|
+
target.segmentId = segmentId;
|
|
935
|
+
target.x = x;
|
|
936
|
+
target.y = y;
|
|
937
|
+
target.distance = distance;
|
|
938
|
+
target.heading = heading;
|
|
939
|
+
target.normalX = normalX;
|
|
940
|
+
target.normalY = normalY;
|
|
941
|
+
target.curvature = curvature;
|
|
942
|
+
target.signedOffset = signedOffset;
|
|
943
|
+
target.crossTrackError = Math.abs(signedOffset);
|
|
944
|
+
target.distanceSquared = distanceSquared;
|
|
945
|
+
return target;
|
|
946
|
+
}
|
|
947
|
+
|
|
948
|
+
function bestProjectionFromHintedSegmentNeighborhood(index, distanceAlong, radius, position, preferredDistance = null, target = null) {
|
|
949
|
+
const centerSegmentId = segmentIdAtDistance(index, distanceAlong);
|
|
950
|
+
if (!Number.isInteger(centerSegmentId)) return { projection: null, reason: 'no-candidates' };
|
|
951
|
+
return bestProjectionFromSegmentNeighborhood(
|
|
952
|
+
index.centerline,
|
|
953
|
+
centerSegmentId,
|
|
954
|
+
radius,
|
|
955
|
+
position,
|
|
956
|
+
preferredDistance,
|
|
957
|
+
target,
|
|
958
|
+
);
|
|
959
|
+
}
|
|
960
|
+
|
|
961
|
+
function segmentIdAtDistance(index, distanceAlong) {
|
|
962
|
+
const centerline = index?.centerline;
|
|
963
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
964
|
+
const totalLength = index?.arcBuckets?.totalLength;
|
|
965
|
+
if (segmentCount <= 0 || !Number.isFinite(distanceAlong) || !Number.isFinite(totalLength) || totalLength <= 0) return null;
|
|
966
|
+
const scratch = ensureQueryScratch(index);
|
|
967
|
+
if (scratch.hintDistanceCacheDistance === distanceAlong && Number.isInteger(scratch.hintDistanceCacheSegmentId) && scratch.hintDistanceCacheSegmentId >= 0) {
|
|
968
|
+
index.stats.hintDistanceCacheHits += 1;
|
|
969
|
+
return scratch.hintDistanceCacheSegmentId;
|
|
970
|
+
}
|
|
971
|
+
const wrapped = wrapDistance(distanceAlong, totalLength);
|
|
972
|
+
let low = 0;
|
|
973
|
+
let high = segmentCount - 1;
|
|
974
|
+
while (low < high) {
|
|
975
|
+
const mid = Math.floor((low + high) / 2);
|
|
976
|
+
if (centerline.endDistance[mid] < wrapped) low = mid + 1;
|
|
977
|
+
else high = mid;
|
|
978
|
+
}
|
|
979
|
+
scratch.hintDistanceCacheDistance = distanceAlong;
|
|
980
|
+
scratch.hintDistanceCacheWrappedDistance = wrapped;
|
|
981
|
+
scratch.hintDistanceCacheSegmentId = low;
|
|
982
|
+
scratch.hintDistanceCachePointX = Number.NaN;
|
|
983
|
+
scratch.hintDistanceCachePointY = Number.NaN;
|
|
984
|
+
return low;
|
|
275
985
|
}
|
|
276
986
|
|
|
277
987
|
function projectionTieScore(projection, preferredDistance) {
|
|
@@ -279,6 +989,37 @@ function projectionTieScore(projection, preferredDistance) {
|
|
|
279
989
|
return Math.abs(projection.distance - preferredDistance);
|
|
280
990
|
}
|
|
281
991
|
|
|
992
|
+
function projectionTieScoreForCandidate(distance, segmentId, preferredDistance) {
|
|
993
|
+
if (!Number.isFinite(preferredDistance)) return segmentId;
|
|
994
|
+
return Math.abs(distance - preferredDistance);
|
|
995
|
+
}
|
|
996
|
+
|
|
997
|
+
function projectionTieBeats(candidate, candidateTieScore, current, currentTieScore) {
|
|
998
|
+
if (candidateTieScore < currentTieScore - AMBIGUOUS_DISTANCE_EPSILON) return true;
|
|
999
|
+
if (Math.abs(candidateTieScore - currentTieScore) > AMBIGUOUS_DISTANCE_EPSILON) return false;
|
|
1000
|
+
return candidate.segmentId < current.segmentId;
|
|
1001
|
+
}
|
|
1002
|
+
|
|
1003
|
+
function projectionTieBeatsCandidate(candidateSegmentId, candidateTieScore, currentSegmentId, currentTieScore) {
|
|
1004
|
+
if (candidateTieScore < currentTieScore - AMBIGUOUS_DISTANCE_EPSILON) return true;
|
|
1005
|
+
if (Math.abs(candidateTieScore - currentTieScore) > AMBIGUOUS_DISTANCE_EPSILON) return false;
|
|
1006
|
+
return candidateSegmentId < currentSegmentId;
|
|
1007
|
+
}
|
|
1008
|
+
|
|
1009
|
+
function ensureFloat64ScratchArray(scratch, key, minimumLength) {
|
|
1010
|
+
if (!(scratch[key] instanceof Float64Array) || scratch[key].length < minimumLength) {
|
|
1011
|
+
scratch[key] = new Float64Array(Math.max(1, minimumLength));
|
|
1012
|
+
}
|
|
1013
|
+
return scratch[key];
|
|
1014
|
+
}
|
|
1015
|
+
|
|
1016
|
+
function ensureUint8ScratchArray(scratch, key, minimumLength) {
|
|
1017
|
+
if (!(scratch[key] instanceof Uint8Array) || scratch[key].length < minimumLength) {
|
|
1018
|
+
scratch[key] = new Uint8Array(Math.max(1, minimumLength));
|
|
1019
|
+
}
|
|
1020
|
+
return scratch[key];
|
|
1021
|
+
}
|
|
1022
|
+
|
|
282
1023
|
export function queryNearbyTrackProjections(track, position, { neighborLimit = GRID_NEIGHBOR_LIMIT } = {}) {
|
|
283
1024
|
const index = track?.queryIndex;
|
|
284
1025
|
if (!index?.centerline?.segmentCount || !finitePoint(position)) return null;
|
|
@@ -296,7 +1037,7 @@ export function queryTrackSegmentsInBounds(track, bounds) {
|
|
|
296
1037
|
return ids.map((segmentId) => segmentFromIndex(index.centerline, segmentId));
|
|
297
1038
|
}
|
|
298
1039
|
|
|
299
|
-
export function
|
|
1040
|
+
export function queryTrackSegmentIdsAlongRay(track, origin, vector, maxDistance, margin = 0) {
|
|
300
1041
|
const index = track?.queryIndex;
|
|
301
1042
|
if (
|
|
302
1043
|
!index?.centerline?.segmentCount ||
|
|
@@ -320,15 +1061,15 @@ export function queryTrackSegmentsAlongRay(track, origin, vector, maxDistance, m
|
|
|
320
1061
|
const sampleCount = Math.max(1, Math.ceil(maxDistance / step));
|
|
321
1062
|
const visitedCells = scratch.rayVisitedCells;
|
|
322
1063
|
visitedCells.length = 0;
|
|
1064
|
+
const point = scratch.rayTracePoint ?? { x: 0, y: 0 };
|
|
1065
|
+
scratch.rayTracePoint = point;
|
|
323
1066
|
|
|
324
1067
|
const seed = seedRayTraceFromNearbyCache(index, grid, origin, vector, radius, maxDistance, segmentMarks, segmentEpoch, cellMarks, cellEpoch, ids);
|
|
325
1068
|
|
|
326
1069
|
for (let sample = 0; sample <= sampleCount; sample += 1) {
|
|
327
1070
|
const distance = Math.min(maxDistance, sample * step);
|
|
328
|
-
|
|
329
|
-
|
|
330
|
-
y: origin.y + vector.y * distance,
|
|
331
|
-
};
|
|
1071
|
+
point.x = origin.x + vector.x * distance;
|
|
1072
|
+
point.y = origin.y + vector.y * distance;
|
|
332
1073
|
const center = gridCellForPoint(grid, point, false);
|
|
333
1074
|
if (!center) continue;
|
|
334
1075
|
for (let row = center.row - radius; row <= center.row + radius; row += 1) {
|
|
@@ -350,6 +1091,13 @@ export function queryTrackSegmentsAlongRay(track, origin, vector, maxDistance, m
|
|
|
350
1091
|
}
|
|
351
1092
|
|
|
352
1093
|
storeRayTraceCache(index, grid, origin, vector, radius, maxDistance, ids, visitedCells, seed);
|
|
1094
|
+
return ids;
|
|
1095
|
+
}
|
|
1096
|
+
|
|
1097
|
+
export function queryTrackSegmentsAlongRay(track, origin, vector, maxDistance, margin = 0) {
|
|
1098
|
+
const index = track?.queryIndex;
|
|
1099
|
+
const ids = queryTrackSegmentIdsAlongRay(track, origin, vector, maxDistance, margin);
|
|
1100
|
+
if (!ids) return null;
|
|
353
1101
|
if (!ids.length) return [];
|
|
354
1102
|
return ids.map((segmentId) => segmentFromIndex(index.centerline, segmentId));
|
|
355
1103
|
}
|
|
@@ -364,13 +1112,16 @@ export function attachTrackQueryIndex(track, queryIndex) {
|
|
|
364
1112
|
return track;
|
|
365
1113
|
}
|
|
366
1114
|
|
|
367
|
-
export function forkTrackQueryIndex(sourceIndex) {
|
|
1115
|
+
export function forkTrackQueryIndex(sourceIndex, runtimeTrack = null) {
|
|
368
1116
|
if (!sourceIndex || typeof sourceIndex !== 'object') return null;
|
|
369
|
-
|
|
1117
|
+
const index = {
|
|
370
1118
|
...sourceIndex,
|
|
1119
|
+
pit: runtimeTrack ? createPitQueryIndex(runtimeTrack, sourceIndex.grid?.cellSize ?? DEFAULT_GRID_CELL_SIZE) : sourceIndex.pit,
|
|
371
1120
|
stats: createQueryStats(),
|
|
372
|
-
queryScratch: createQueryScratch(sourceIndex),
|
|
373
1121
|
};
|
|
1122
|
+
attachQueryStatsToCenterline(index.centerline, index.stats);
|
|
1123
|
+
index.queryScratch = createQueryScratch(index);
|
|
1124
|
+
return index;
|
|
374
1125
|
}
|
|
375
1126
|
|
|
376
1127
|
export function resetTrackQueryStats(track) {
|
|
@@ -385,6 +1136,20 @@ export function snapshotTrackQueryStats(track) {
|
|
|
385
1136
|
return stats ? JSON.parse(JSON.stringify(stats)) : null;
|
|
386
1137
|
}
|
|
387
1138
|
|
|
1139
|
+
function attachQueryStatsToCenterline(centerline, stats) {
|
|
1140
|
+
const descriptor = Object.getOwnPropertyDescriptor(centerline, 'stats');
|
|
1141
|
+
if (!descriptor) {
|
|
1142
|
+
Object.defineProperty(centerline, 'stats', {
|
|
1143
|
+
configurable: true,
|
|
1144
|
+
enumerable: false,
|
|
1145
|
+
value: stats,
|
|
1146
|
+
writable: true,
|
|
1147
|
+
});
|
|
1148
|
+
return;
|
|
1149
|
+
}
|
|
1150
|
+
if (descriptor.writable) centerline.stats = stats;
|
|
1151
|
+
}
|
|
1152
|
+
|
|
388
1153
|
function createTrackBands(track) {
|
|
389
1154
|
const trackEdge = track.width / 2;
|
|
390
1155
|
const kerbEdge = trackEdge + (track.kerbWidth ?? 0);
|
|
@@ -405,8 +1170,12 @@ function createCenterlineSegments(samples, segmentCount) {
|
|
|
405
1170
|
const startY = new Float64Array(segmentCount);
|
|
406
1171
|
const endX = new Float64Array(segmentCount);
|
|
407
1172
|
const endY = new Float64Array(segmentCount);
|
|
1173
|
+
const deltaX = new Float64Array(segmentCount);
|
|
1174
|
+
const deltaY = new Float64Array(segmentCount);
|
|
1175
|
+
const lengthSquared = new Float64Array(segmentCount);
|
|
408
1176
|
const startDistance = new Float64Array(segmentCount);
|
|
409
1177
|
const endDistance = new Float64Array(segmentCount);
|
|
1178
|
+
const distanceSpan = new Float64Array(segmentCount);
|
|
410
1179
|
const heading = new Float64Array(segmentCount);
|
|
411
1180
|
const normalX = new Float64Array(segmentCount);
|
|
412
1181
|
const normalY = new Float64Array(segmentCount);
|
|
@@ -425,8 +1194,12 @@ function createCenterlineSegments(samples, segmentCount) {
|
|
|
425
1194
|
startY[id] = start.y;
|
|
426
1195
|
endX[id] = end.x;
|
|
427
1196
|
endY[id] = end.y;
|
|
1197
|
+
deltaX[id] = end.x - start.x;
|
|
1198
|
+
deltaY[id] = end.y - start.y;
|
|
1199
|
+
lengthSquared[id] = deltaX[id] * deltaX[id] + deltaY[id] * deltaY[id];
|
|
428
1200
|
startDistance[id] = start.distance;
|
|
429
1201
|
endDistance[id] = end.distance;
|
|
1202
|
+
distanceSpan[id] = end.distance - start.distance;
|
|
430
1203
|
heading[id] = start.heading;
|
|
431
1204
|
normalX[id] = start.normalX;
|
|
432
1205
|
normalY[id] = start.normalY;
|
|
@@ -445,8 +1218,12 @@ function createCenterlineSegments(samples, segmentCount) {
|
|
|
445
1218
|
startY,
|
|
446
1219
|
endX,
|
|
447
1220
|
endY,
|
|
1221
|
+
deltaX,
|
|
1222
|
+
deltaY,
|
|
1223
|
+
lengthSquared,
|
|
448
1224
|
startDistance,
|
|
449
1225
|
endDistance,
|
|
1226
|
+
distanceSpan,
|
|
450
1227
|
heading,
|
|
451
1228
|
normalX,
|
|
452
1229
|
normalY,
|
|
@@ -460,12 +1237,36 @@ function createCenterlineSegments(samples, segmentCount) {
|
|
|
460
1237
|
};
|
|
461
1238
|
}
|
|
462
1239
|
|
|
1240
|
+
function createPrecomputedSegmentNeighborhoods(segmentCount, radii) {
|
|
1241
|
+
if (!segmentCount || !radii?.size) return new Map();
|
|
1242
|
+
const neighborhoodsByRadius = new Map();
|
|
1243
|
+
for (const radius of radii) {
|
|
1244
|
+
const width = radius * 2 + 1;
|
|
1245
|
+
const neighborhoods = new Array(segmentCount);
|
|
1246
|
+
for (let centerSegmentId = 0; centerSegmentId < segmentCount; centerSegmentId += 1) {
|
|
1247
|
+
const segmentIds = new Int32Array(width);
|
|
1248
|
+
for (let index = 0; index < width; index += 1) {
|
|
1249
|
+
segmentIds[index] = ((centerSegmentId + (index - radius)) % segmentCount + segmentCount) % segmentCount;
|
|
1250
|
+
}
|
|
1251
|
+
neighborhoods[centerSegmentId] = segmentIds;
|
|
1252
|
+
}
|
|
1253
|
+
neighborhoodsByRadius.set(radius, neighborhoods);
|
|
1254
|
+
}
|
|
1255
|
+
return neighborhoodsByRadius;
|
|
1256
|
+
}
|
|
1257
|
+
|
|
1258
|
+
function getPrecomputedSegmentNeighborhood(centerline, centerSegmentId, radius) {
|
|
1259
|
+
const neighborhoods = centerline?.precomputedNeighborhoodsByRadius?.get(radius);
|
|
1260
|
+
return neighborhoods?.[centerSegmentId] ?? null;
|
|
1261
|
+
}
|
|
1262
|
+
|
|
463
1263
|
function createArcBuckets(totalLength, count) {
|
|
464
1264
|
return {
|
|
465
1265
|
count,
|
|
466
1266
|
totalLength,
|
|
467
1267
|
bucketLength: totalLength / count,
|
|
468
1268
|
buckets: Array.from({ length: count }, () => []),
|
|
1269
|
+
radius2CandidateIdsByBucket: Array.from({ length: count }, () => []),
|
|
469
1270
|
};
|
|
470
1271
|
}
|
|
471
1272
|
|
|
@@ -478,6 +1279,29 @@ function insertSegmentIntoArcBuckets(arcBuckets, centerline, segmentId) {
|
|
|
478
1279
|
}
|
|
479
1280
|
}
|
|
480
1281
|
|
|
1282
|
+
function finalizeArcBuckets(arcBuckets, segmentCount) {
|
|
1283
|
+
if (!arcBuckets?.count || !segmentCount) return;
|
|
1284
|
+
const candidateMarks = new Uint32Array(Math.max(1, segmentCount));
|
|
1285
|
+
let epoch = 0;
|
|
1286
|
+
for (let center = 0; center < arcBuckets.count; center += 1) {
|
|
1287
|
+
epoch += 1;
|
|
1288
|
+
if (epoch === 0xffffffff) {
|
|
1289
|
+
candidateMarks.fill(0);
|
|
1290
|
+
epoch = 1;
|
|
1291
|
+
}
|
|
1292
|
+
const candidateIds = [];
|
|
1293
|
+
for (let offset = -2; offset <= 2; offset += 1) {
|
|
1294
|
+
const bucket = ((center + offset) % arcBuckets.count + arcBuckets.count) % arcBuckets.count;
|
|
1295
|
+
for (const segmentId of arcBuckets.buckets[bucket]) {
|
|
1296
|
+
if (candidateMarks[segmentId] === epoch) continue;
|
|
1297
|
+
candidateMarks[segmentId] = epoch;
|
|
1298
|
+
candidateIds.push(segmentId);
|
|
1299
|
+
}
|
|
1300
|
+
}
|
|
1301
|
+
arcBuckets.radius2CandidateIdsByBucket[center] = candidateIds;
|
|
1302
|
+
}
|
|
1303
|
+
}
|
|
1304
|
+
|
|
481
1305
|
function expandedSampleBounds(samples, expansion) {
|
|
482
1306
|
const bounds = samples.reduce((current, point) => ({
|
|
483
1307
|
minX: Math.min(current.minX, point.x),
|
|
@@ -502,41 +1326,471 @@ function insertSegmentIntoGrid(grid, centerline, segmentId, expansion) {
|
|
|
502
1326
|
});
|
|
503
1327
|
}
|
|
504
1328
|
|
|
505
|
-
function
|
|
506
|
-
const
|
|
507
|
-
|
|
1329
|
+
function createNearestNonLocalHintDistanceSquared(centerline, grid, maxDistance) {
|
|
1330
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
1331
|
+
const distances = new Float64Array(segmentCount);
|
|
1332
|
+
distances.fill(Infinity);
|
|
1333
|
+
if (!segmentCount || !grid) return distances;
|
|
1334
|
+
const candidateMarks = new Uint32Array(Math.max(1, segmentCount));
|
|
1335
|
+
let epoch = 0;
|
|
1336
|
+
const maxDistanceSquared = maxDistance * maxDistance;
|
|
1337
|
+
for (let segmentId = 0; segmentId < segmentCount; segmentId += 1) {
|
|
1338
|
+
epoch += 1;
|
|
1339
|
+
if (epoch === 0xffffffff) {
|
|
1340
|
+
candidateMarks.fill(0);
|
|
1341
|
+
epoch = 1;
|
|
1342
|
+
}
|
|
1343
|
+
const bounds = {
|
|
1344
|
+
minX: centerline.minX[segmentId] - maxDistance,
|
|
1345
|
+
maxX: centerline.maxX[segmentId] + maxDistance,
|
|
1346
|
+
minY: centerline.minY[segmentId] - maxDistance,
|
|
1347
|
+
maxY: centerline.maxY[segmentId] + maxDistance,
|
|
1348
|
+
};
|
|
1349
|
+
const minCell = gridCellForPoint(grid, { x: bounds.minX, y: bounds.minY }, true);
|
|
1350
|
+
const maxCell = gridCellForPoint(grid, { x: bounds.maxX, y: bounds.maxY }, true);
|
|
1351
|
+
let bestDistanceSquared = Infinity;
|
|
1352
|
+
for (let row = minCell.row; row <= maxCell.row; row += 1) {
|
|
1353
|
+
for (let column = minCell.column; column <= maxCell.column; column += 1) {
|
|
1354
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1355
|
+
if (!cell) continue;
|
|
1356
|
+
for (const candidateId of cell) {
|
|
1357
|
+
if (candidateMarks[candidateId] === epoch) continue;
|
|
1358
|
+
candidateMarks[candidateId] = epoch;
|
|
1359
|
+
if (isLocalSegmentNeighbor(segmentId, candidateId, segmentCount, 2)) continue;
|
|
1360
|
+
const candidateDistanceSquared = segmentDistanceSquared(centerline, segmentId, candidateId);
|
|
1361
|
+
if (candidateDistanceSquared < bestDistanceSquared) {
|
|
1362
|
+
bestDistanceSquared = candidateDistanceSquared;
|
|
1363
|
+
}
|
|
1364
|
+
if (bestDistanceSquared <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1365
|
+
break;
|
|
1366
|
+
}
|
|
1367
|
+
}
|
|
1368
|
+
if (bestDistanceSquared <= AMBIGUOUS_DISTANCE_EPSILON) break;
|
|
1369
|
+
}
|
|
1370
|
+
if (bestDistanceSquared <= AMBIGUOUS_DISTANCE_EPSILON) break;
|
|
1371
|
+
}
|
|
1372
|
+
distances[segmentId] = bestDistanceSquared <= maxDistanceSquared ? bestDistanceSquared : Infinity;
|
|
1373
|
+
}
|
|
1374
|
+
return distances;
|
|
1375
|
+
}
|
|
1376
|
+
|
|
1377
|
+
function createLowDensityHintRing1Neighborhood(centerline, grid, threshold) {
|
|
1378
|
+
const cellCount = grid?.columns * grid?.rows;
|
|
1379
|
+
if (!centerline?.segmentCount || !grid || !Number.isInteger(cellCount) || cellCount <= 0) {
|
|
1380
|
+
return {
|
|
1381
|
+
counts: new Uint16Array(0),
|
|
1382
|
+
candidateIdsByCell: [],
|
|
1383
|
+
};
|
|
1384
|
+
}
|
|
1385
|
+
const counts = new Uint16Array(cellCount);
|
|
1386
|
+
const candidateIdsByCell = new Array(cellCount).fill(null);
|
|
1387
|
+
const candidateMarks = new Uint32Array(Math.max(1, centerline.segmentCount));
|
|
1388
|
+
let epoch = 0;
|
|
1389
|
+
for (let row = 0; row < grid.rows; row += 1) {
|
|
1390
|
+
for (let column = 0; column < grid.columns; column += 1) {
|
|
1391
|
+
epoch += 1;
|
|
1392
|
+
if (epoch === 0xffffffff) {
|
|
1393
|
+
candidateMarks.fill(0);
|
|
1394
|
+
epoch = 1;
|
|
1395
|
+
}
|
|
1396
|
+
let uniqueCount = 0;
|
|
1397
|
+
const ids = [];
|
|
1398
|
+
for (let localRow = Math.max(0, row - 1); localRow <= Math.min(grid.rows - 1, row + 1); localRow += 1) {
|
|
1399
|
+
for (
|
|
1400
|
+
let localColumn = Math.max(0, column - 1);
|
|
1401
|
+
localColumn <= Math.min(grid.columns - 1, column + 1);
|
|
1402
|
+
localColumn += 1
|
|
1403
|
+
) {
|
|
1404
|
+
const cell = grid.cells[localRow * grid.columns + localColumn];
|
|
1405
|
+
if (!cell) continue;
|
|
1406
|
+
for (const segmentId of cell) {
|
|
1407
|
+
if (candidateMarks[segmentId] === epoch) continue;
|
|
1408
|
+
candidateMarks[segmentId] = epoch;
|
|
1409
|
+
uniqueCount += 1;
|
|
1410
|
+
ids.push(segmentId);
|
|
1411
|
+
}
|
|
1412
|
+
}
|
|
1413
|
+
}
|
|
1414
|
+
const cellIndex = row * grid.columns + column;
|
|
1415
|
+
counts[cellIndex] = uniqueCount;
|
|
1416
|
+
if (uniqueCount <= threshold) {
|
|
1417
|
+
candidateIdsByCell[cellIndex] = ids;
|
|
1418
|
+
}
|
|
1419
|
+
}
|
|
1420
|
+
}
|
|
1421
|
+
return { counts, candidateIdsByCell };
|
|
1422
|
+
}
|
|
1423
|
+
|
|
1424
|
+
function createNearestOccupiedCellRadiusByCell(grid) {
|
|
1425
|
+
const cellCount = grid?.columns * grid?.rows;
|
|
1426
|
+
if (!grid || !Number.isInteger(cellCount) || cellCount <= 0) {
|
|
1427
|
+
return new Uint16Array(0);
|
|
1428
|
+
}
|
|
1429
|
+
const radii = new Uint16Array(cellCount);
|
|
1430
|
+
radii.fill(0xffff);
|
|
1431
|
+
const queue = new Uint32Array(cellCount);
|
|
1432
|
+
let head = 0;
|
|
1433
|
+
let tail = 0;
|
|
1434
|
+
|
|
1435
|
+
for (let cellIndex = 0; cellIndex < cellCount; cellIndex += 1) {
|
|
1436
|
+
if (!grid.cells[cellIndex]?.length) continue;
|
|
1437
|
+
radii[cellIndex] = 0;
|
|
1438
|
+
queue[tail] = cellIndex;
|
|
1439
|
+
tail += 1;
|
|
1440
|
+
}
|
|
1441
|
+
|
|
1442
|
+
while (head < tail) {
|
|
1443
|
+
const cellIndex = queue[head];
|
|
1444
|
+
head += 1;
|
|
1445
|
+
const row = Math.floor(cellIndex / grid.columns);
|
|
1446
|
+
const column = cellIndex - row * grid.columns;
|
|
1447
|
+
const nextRadius = radii[cellIndex] + 1;
|
|
1448
|
+
|
|
1449
|
+
for (let rowOffset = -1; rowOffset <= 1; rowOffset += 1) {
|
|
1450
|
+
const neighborRow = row + rowOffset;
|
|
1451
|
+
if (neighborRow < 0 || neighborRow >= grid.rows) continue;
|
|
1452
|
+
for (let columnOffset = -1; columnOffset <= 1; columnOffset += 1) {
|
|
1453
|
+
const neighborColumn = column + columnOffset;
|
|
1454
|
+
if (neighborColumn < 0 || neighborColumn >= grid.columns) continue;
|
|
1455
|
+
const neighborIndex = neighborRow * grid.columns + neighborColumn;
|
|
1456
|
+
if (radii[neighborIndex] <= nextRadius) continue;
|
|
1457
|
+
radii[neighborIndex] = nextRadius;
|
|
1458
|
+
queue[tail] = neighborIndex;
|
|
1459
|
+
tail += 1;
|
|
1460
|
+
}
|
|
1461
|
+
}
|
|
1462
|
+
}
|
|
1463
|
+
|
|
1464
|
+
return radii;
|
|
1465
|
+
}
|
|
1466
|
+
|
|
1467
|
+
function isLocalSegmentNeighbor(segmentId, candidateId, segmentCount, radius) {
|
|
1468
|
+
const delta = Math.abs(segmentId - candidateId);
|
|
1469
|
+
return Math.min(delta, segmentCount - delta) <= radius;
|
|
1470
|
+
}
|
|
1471
|
+
|
|
1472
|
+
function segmentDistanceSquared(centerline, firstSegmentId, secondSegmentId) {
|
|
1473
|
+
const ax = centerline.startX[firstSegmentId];
|
|
1474
|
+
const ay = centerline.startY[firstSegmentId];
|
|
1475
|
+
const bx = centerline.endX[firstSegmentId];
|
|
1476
|
+
const by = centerline.endY[firstSegmentId];
|
|
1477
|
+
const cx = centerline.startX[secondSegmentId];
|
|
1478
|
+
const cy = centerline.startY[secondSegmentId];
|
|
1479
|
+
const dx = centerline.endX[secondSegmentId];
|
|
1480
|
+
const dy = centerline.endY[secondSegmentId];
|
|
1481
|
+
|
|
1482
|
+
if (segmentsIntersect(ax, ay, bx, by, cx, cy, dx, dy)) return 0;
|
|
1483
|
+
|
|
1484
|
+
return Math.min(
|
|
1485
|
+
pointToSegmentDistanceSquared(ax, ay, cx, cy, dx, dy),
|
|
1486
|
+
pointToSegmentDistanceSquared(bx, by, cx, cy, dx, dy),
|
|
1487
|
+
pointToSegmentDistanceSquared(cx, cy, ax, ay, bx, by),
|
|
1488
|
+
pointToSegmentDistanceSquared(dx, dy, ax, ay, bx, by),
|
|
1489
|
+
);
|
|
1490
|
+
}
|
|
1491
|
+
|
|
1492
|
+
function isHintProjectionDistanceSafe(index, projection) {
|
|
1493
|
+
const safeDistanceSquared = index.nearestNonLocalHintDistanceSquared?.[projection.segmentId] ?? 0;
|
|
1494
|
+
if (!Number.isFinite(safeDistanceSquared)) return true;
|
|
1495
|
+
return 4 * projection.crossTrackError * projection.crossTrackError < safeDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON;
|
|
1496
|
+
}
|
|
1497
|
+
|
|
1498
|
+
function hintedQueryStaysInLocalCorridor(index, position, progressHint, maxOffset) {
|
|
1499
|
+
const hintPoint = pointAtIndexedDistance(index, progressHint);
|
|
1500
|
+
if (!hintPoint) return false;
|
|
1501
|
+
const dx = position.x - hintPoint.x;
|
|
1502
|
+
const dy = position.y - hintPoint.y;
|
|
1503
|
+
return dx * dx + dy * dy <= maxOffset * maxOffset + AMBIGUOUS_DISTANCE_EPSILON;
|
|
1504
|
+
}
|
|
1505
|
+
|
|
1506
|
+
function pointAtIndexedDistance(index, distanceAlong) {
|
|
1507
|
+
const centerline = index?.centerline;
|
|
1508
|
+
const scratch = ensureQueryScratch(index);
|
|
1509
|
+
const segmentId = segmentIdAtDistance(index, distanceAlong);
|
|
1510
|
+
if (!centerline || !Number.isInteger(segmentId)) return null;
|
|
1511
|
+
if (
|
|
1512
|
+
scratch.hintDistanceCacheDistance === distanceAlong &&
|
|
1513
|
+
Number.isFinite(scratch.hintDistanceCachePointX) &&
|
|
1514
|
+
Number.isFinite(scratch.hintDistanceCachePointY)
|
|
1515
|
+
) {
|
|
1516
|
+
return {
|
|
1517
|
+
x: scratch.hintDistanceCachePointX,
|
|
1518
|
+
y: scratch.hintDistanceCachePointY,
|
|
1519
|
+
};
|
|
1520
|
+
}
|
|
1521
|
+
const startDistance = centerline.startDistance[segmentId];
|
|
1522
|
+
const endDistance = centerline.endDistance[segmentId];
|
|
1523
|
+
const span = Math.max(AMBIGUOUS_DISTANCE_EPSILON, endDistance - startDistance);
|
|
1524
|
+
const wrappedDistance = scratch.hintDistanceCacheDistance === distanceAlong &&
|
|
1525
|
+
Number.isFinite(scratch.hintDistanceCacheWrappedDistance)
|
|
1526
|
+
? scratch.hintDistanceCacheWrappedDistance
|
|
1527
|
+
: wrapDistance(distanceAlong, index.arcBuckets.totalLength);
|
|
1528
|
+
const amount = clamp((wrappedDistance - startDistance) / span, 0, 1);
|
|
1529
|
+
const point = {
|
|
1530
|
+
x: centerline.startX[segmentId] + (centerline.endX[segmentId] - centerline.startX[segmentId]) * amount,
|
|
1531
|
+
y: centerline.startY[segmentId] + (centerline.endY[segmentId] - centerline.startY[segmentId]) * amount,
|
|
1532
|
+
};
|
|
1533
|
+
scratch.hintDistanceCacheDistance = distanceAlong;
|
|
1534
|
+
scratch.hintDistanceCacheWrappedDistance = wrappedDistance;
|
|
1535
|
+
scratch.hintDistanceCacheSegmentId = segmentId;
|
|
1536
|
+
scratch.hintDistanceCachePointX = point.x;
|
|
1537
|
+
scratch.hintDistanceCachePointY = point.y;
|
|
1538
|
+
return point;
|
|
1539
|
+
}
|
|
1540
|
+
|
|
1541
|
+
function isHintProjectionLowDensitySafe(index, position) {
|
|
1542
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1543
|
+
const counts = index?.lowDensityHintRing1SegmentCounts;
|
|
1544
|
+
if (!grid || !counts?.length || !finitePoint(position)) return false;
|
|
1545
|
+
const cell = gridCellForPoint(grid, position, false);
|
|
1546
|
+
if (!cell) return false;
|
|
1547
|
+
const count = counts[cell.row * grid.columns + cell.column] ?? Infinity;
|
|
1548
|
+
return count <= LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD;
|
|
1549
|
+
}
|
|
1550
|
+
|
|
1551
|
+
function bestProjectionFromLowDensityCellNeighborhood(index, position, preferredDistance = null, target = null) {
|
|
1552
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1553
|
+
const counts = index?.lowDensityHintRing1SegmentCounts;
|
|
1554
|
+
const candidateIdsByCell = index?.lowDensityHintRing1CandidateIdsByCell;
|
|
1555
|
+
if (!grid || !counts?.length || !candidateIdsByCell?.length || !finitePoint(position)) {
|
|
1556
|
+
return { projection: null, reason: 'no-candidates' };
|
|
1557
|
+
}
|
|
1558
|
+
const center = gridCellForPoint(grid, position, false);
|
|
1559
|
+
if (!center) return { projection: null, reason: 'no-candidates' };
|
|
1560
|
+
const cellIndex = center.row * grid.columns + center.column;
|
|
1561
|
+
if ((counts[cellIndex] ?? Infinity) > LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD) {
|
|
1562
|
+
return { projection: null, reason: 'no-candidates' };
|
|
1563
|
+
}
|
|
1564
|
+
const candidateIds = candidateIdsByCell[cellIndex];
|
|
1565
|
+
if (!candidateIds?.length) return { projection: null, reason: 'no-candidates' };
|
|
1566
|
+
const local = bestProjectionFromCandidates(index.centerline, candidateIds, position, preferredDistance, target);
|
|
1567
|
+
if (!local.projection) return local;
|
|
1568
|
+
forEachRingCell(grid, center, 2, (_ringCellIndex, row, column) => {
|
|
1569
|
+
if (local.projection == null) return;
|
|
1570
|
+
if (
|
|
1571
|
+
row >= center.row - 1 &&
|
|
1572
|
+
row <= center.row + 1 &&
|
|
1573
|
+
column >= center.column - 1 &&
|
|
1574
|
+
column <= center.column + 1
|
|
1575
|
+
) return;
|
|
1576
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1577
|
+
if (!cell?.length) return;
|
|
1578
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
1579
|
+
if (lowerBound <= local.projection.distanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1580
|
+
local.projection = null;
|
|
1581
|
+
}
|
|
1582
|
+
});
|
|
1583
|
+
return local.projection ? local : { projection: null, reason: 'needs-wider-search' };
|
|
1584
|
+
}
|
|
1585
|
+
|
|
1586
|
+
function bestProjectionFromRing2Neighborhood(index, position, preferredDistance = null, target = null) {
|
|
1587
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1588
|
+
if (!grid || !finitePoint(position)) {
|
|
1589
|
+
return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1590
|
+
}
|
|
1591
|
+
const center = gridCellForPoint(grid, position, false);
|
|
1592
|
+
if (!center) return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1593
|
+
const candidateIds = candidateIdsFromCellNeighborhood(index, grid, center, 2);
|
|
1594
|
+
if (!candidateIds.length) return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1595
|
+
const local = bestProjectionFromCandidates(index.centerline, candidateIds, position, preferredDistance, target);
|
|
1596
|
+
if (!local.projection) return { projection: null, bestProjection: null, reason: local.reason ?? 'no-candidates' };
|
|
1597
|
+
const bestProjection = local.projection;
|
|
1598
|
+
forEachRingCell(grid, center, 3, (_cellIndex, row, column) => {
|
|
1599
|
+
if (local.projection == null) return;
|
|
1600
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1601
|
+
if (!cell?.length) return;
|
|
1602
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
1603
|
+
if (lowerBound <= local.projection.distanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1604
|
+
local.projection = null;
|
|
1605
|
+
}
|
|
1606
|
+
});
|
|
1607
|
+
return local.projection
|
|
1608
|
+
? local
|
|
1609
|
+
: { projection: null, bestProjection, reason: 'needs-wider-search' };
|
|
1610
|
+
}
|
|
1611
|
+
|
|
1612
|
+
function candidateIdsFromCellNeighborhood(index, grid, center, radius) {
|
|
508
1613
|
const scratch = ensureQueryScratch(index);
|
|
509
|
-
const segmentMarks = ensureScratchArray(scratch, '
|
|
510
|
-
const
|
|
1614
|
+
const segmentMarks = ensureScratchArray(scratch, 'cellNeighborhoodSegmentMarks', index.centerline.segmentCount);
|
|
1615
|
+
const segmentEpoch = nextScratchEpoch(scratch, 'cellNeighborhoodSegmentEpoch', segmentMarks);
|
|
1616
|
+
const ids = scratch.cellNeighborhoodSegmentIds ?? [];
|
|
1617
|
+
ids.length = 0;
|
|
1618
|
+
for (let row = Math.max(0, center.row - radius); row <= Math.min(grid.rows - 1, center.row + radius); row += 1) {
|
|
1619
|
+
for (let column = Math.max(0, center.column - radius); column <= Math.min(grid.columns - 1, center.column + radius); column += 1) {
|
|
1620
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1621
|
+
if (!cell) continue;
|
|
1622
|
+
for (const segmentId of cell) {
|
|
1623
|
+
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
1624
|
+
segmentMarks[segmentId] = segmentEpoch;
|
|
1625
|
+
ids.push(segmentId);
|
|
1626
|
+
}
|
|
1627
|
+
}
|
|
1628
|
+
}
|
|
1629
|
+
scratch.cellNeighborhoodSegmentIds = ids;
|
|
1630
|
+
return ids;
|
|
1631
|
+
}
|
|
1632
|
+
|
|
1633
|
+
function pointToSegmentDistanceSquared(px, py, ax, ay, bx, by) {
|
|
1634
|
+
const dx = bx - ax;
|
|
1635
|
+
const dy = by - ay;
|
|
1636
|
+
const lengthSquared = dx * dx + dy * dy;
|
|
1637
|
+
const amount = lengthSquared > 0
|
|
1638
|
+
? clamp(((px - ax) * dx + (py - ay) * dy) / lengthSquared, 0, 1)
|
|
1639
|
+
: 0;
|
|
1640
|
+
const x = ax + dx * amount;
|
|
1641
|
+
const y = ay + dy * amount;
|
|
1642
|
+
const deltaX = px - x;
|
|
1643
|
+
const deltaY = py - y;
|
|
1644
|
+
return deltaX * deltaX + deltaY * deltaY;
|
|
1645
|
+
}
|
|
1646
|
+
|
|
1647
|
+
function segmentsIntersect(ax, ay, bx, by, cx, cy, dx, dy) {
|
|
1648
|
+
const abx = bx - ax;
|
|
1649
|
+
const aby = by - ay;
|
|
1650
|
+
const acx = cx - ax;
|
|
1651
|
+
const acy = cy - ay;
|
|
1652
|
+
const adx = dx - ax;
|
|
1653
|
+
const ady = dy - ay;
|
|
1654
|
+
const cdx = dx - cx;
|
|
1655
|
+
const cdy = dy - cy;
|
|
1656
|
+
const cax = ax - cx;
|
|
1657
|
+
const cay = ay - cy;
|
|
1658
|
+
const cbx = bx - cx;
|
|
1659
|
+
const cby = by - cy;
|
|
1660
|
+
const cross1 = abx * acy - aby * acx;
|
|
1661
|
+
const cross2 = abx * ady - aby * adx;
|
|
1662
|
+
const cross3 = cdx * cay - cdy * cax;
|
|
1663
|
+
const cross4 = cdx * cby - cdy * cbx;
|
|
1664
|
+
return cross1 * cross2 <= 0 && cross3 * cross4 <= 0;
|
|
1665
|
+
}
|
|
1666
|
+
|
|
1667
|
+
function bestProjectionFromArcBuckets(index, distanceAlong, radius, position, preferredDistance = null, target = null) {
|
|
1668
|
+
const arcBuckets = index?.arcBuckets;
|
|
1669
|
+
if (!arcBuckets?.count || !Number.isFinite(distanceAlong)) return { projection: null, reason: 'no-candidates' };
|
|
511
1670
|
const wrapped = wrapDistance(distanceAlong, arcBuckets.totalLength);
|
|
512
1671
|
const center = Math.floor(wrapped / arcBuckets.bucketLength);
|
|
513
|
-
const
|
|
514
|
-
|
|
515
|
-
|
|
516
|
-
|
|
517
|
-
|
|
1672
|
+
const precomputedRadius2Candidates = radius === 2
|
|
1673
|
+
? arcBuckets.radius2CandidateIdsByBucket?.[center] ?? null
|
|
1674
|
+
: null;
|
|
1675
|
+
const scratch = precomputedRadius2Candidates ? null : ensureQueryScratch(index);
|
|
1676
|
+
const segmentMarks = precomputedRadius2Candidates
|
|
1677
|
+
? null
|
|
1678
|
+
: ensureScratchArray(scratch, 'candidateMarks', index.centerline.segmentCount);
|
|
1679
|
+
const candidateEpoch = precomputedRadius2Candidates
|
|
1680
|
+
? 0
|
|
1681
|
+
: nextScratchEpoch(scratch, 'candidateEpoch', segmentMarks);
|
|
1682
|
+
let bestSegmentId = null;
|
|
1683
|
+
let bestX = 0;
|
|
1684
|
+
let bestY = 0;
|
|
1685
|
+
let bestDistance = 0;
|
|
1686
|
+
let bestHeading = 0;
|
|
1687
|
+
let bestNormalX = 0;
|
|
1688
|
+
let bestNormalY = 0;
|
|
1689
|
+
let bestCurvature = 0;
|
|
1690
|
+
let bestSignedOffset = 0;
|
|
1691
|
+
let bestDistanceSquared = Infinity;
|
|
1692
|
+
let bestTieScore = Infinity;
|
|
1693
|
+
let secondBestDistance = Infinity;
|
|
1694
|
+
let tieResolved = false;
|
|
1695
|
+
|
|
1696
|
+
const visitCandidate = (segmentId) => {
|
|
1697
|
+
if (segmentMarks) {
|
|
1698
|
+
if (segmentMarks[segmentId] === candidateEpoch) return;
|
|
518
1699
|
segmentMarks[segmentId] = candidateEpoch;
|
|
519
|
-
|
|
1700
|
+
}
|
|
1701
|
+
const ax = index.centerline.startX[segmentId];
|
|
1702
|
+
const ay = index.centerline.startY[segmentId];
|
|
1703
|
+
const dx = index.centerline.deltaX[segmentId];
|
|
1704
|
+
const dy = index.centerline.deltaY[segmentId];
|
|
1705
|
+
const amount = index.centerline.lengthSquared[segmentId] > 0
|
|
1706
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / index.centerline.lengthSquared[segmentId], 0, 1)
|
|
1707
|
+
: 0;
|
|
1708
|
+
const x = ax + dx * amount;
|
|
1709
|
+
const y = ay + dy * amount;
|
|
1710
|
+
const distance = index.centerline.startDistance[segmentId] +
|
|
1711
|
+
index.centerline.distanceSpan[segmentId] * amount;
|
|
1712
|
+
const normalX = index.centerline.normalX[segmentId];
|
|
1713
|
+
const normalY = index.centerline.normalY[segmentId];
|
|
1714
|
+
const px = position.x - x;
|
|
1715
|
+
const py = position.y - y;
|
|
1716
|
+
const signedOffset = px * normalX + py * normalY;
|
|
1717
|
+
const distanceSquared = px * px + py * py;
|
|
1718
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
1719
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1720
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
1721
|
+
bestSegmentId = segmentId;
|
|
1722
|
+
bestX = x;
|
|
1723
|
+
bestY = y;
|
|
1724
|
+
bestDistance = distance;
|
|
1725
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
1726
|
+
bestNormalX = normalX;
|
|
1727
|
+
bestNormalY = normalY;
|
|
1728
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
1729
|
+
bestSignedOffset = signedOffset;
|
|
1730
|
+
bestDistanceSquared = distanceSquared;
|
|
1731
|
+
bestTieScore = tieScore;
|
|
1732
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1733
|
+
tieResolved = true;
|
|
1734
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
1735
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
1736
|
+
bestSegmentId = segmentId;
|
|
1737
|
+
bestX = x;
|
|
1738
|
+
bestY = y;
|
|
1739
|
+
bestDistance = distance;
|
|
1740
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
1741
|
+
bestNormalX = normalX;
|
|
1742
|
+
bestNormalY = normalY;
|
|
1743
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
1744
|
+
bestSignedOffset = signedOffset;
|
|
1745
|
+
bestDistanceSquared = distanceSquared;
|
|
1746
|
+
bestTieScore = tieScore;
|
|
1747
|
+
}
|
|
1748
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
1749
|
+
secondBestDistance = distanceSquared;
|
|
1750
|
+
}
|
|
1751
|
+
};
|
|
1752
|
+
|
|
1753
|
+
if (precomputedRadius2Candidates) {
|
|
1754
|
+
index.stats.arcBucketRadius2PrecomputedQueries += 1;
|
|
1755
|
+
for (const segmentId of precomputedRadius2Candidates) visitCandidate(segmentId);
|
|
1756
|
+
} else {
|
|
1757
|
+
for (let offset = -radius; offset <= radius; offset += 1) {
|
|
1758
|
+
const bucket = ((center + offset) % arcBuckets.count + arcBuckets.count) % arcBuckets.count;
|
|
1759
|
+
for (const segmentId of arcBuckets.buckets[bucket]) visitCandidate(segmentId);
|
|
520
1760
|
}
|
|
521
1761
|
}
|
|
522
|
-
|
|
1762
|
+
|
|
1763
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
1764
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
1765
|
+
segmentId: bestSegmentId,
|
|
1766
|
+
x: bestX,
|
|
1767
|
+
y: bestY,
|
|
1768
|
+
distance: bestDistance,
|
|
1769
|
+
heading: bestHeading,
|
|
1770
|
+
normalX: bestNormalX,
|
|
1771
|
+
normalY: bestNormalY,
|
|
1772
|
+
curvature: bestCurvature,
|
|
1773
|
+
signedOffset: bestSignedOffset,
|
|
1774
|
+
distanceSquared: bestDistanceSquared,
|
|
1775
|
+
});
|
|
1776
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
1777
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
1778
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
523
1779
|
}
|
|
524
1780
|
|
|
525
1781
|
function projectIndexedSegment(centerline, segmentId, position) {
|
|
1782
|
+
if (centerline?.stats) centerline.stats.candidateProjectionObjectAllocations += 1;
|
|
526
1783
|
const ax = centerline.startX[segmentId];
|
|
527
1784
|
const ay = centerline.startY[segmentId];
|
|
528
|
-
const
|
|
529
|
-
const
|
|
530
|
-
const
|
|
531
|
-
|
|
532
|
-
const lengthSquared = dx * dx + dy * dy;
|
|
533
|
-
const amount = lengthSquared > 0
|
|
534
|
-
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / lengthSquared, 0, 1)
|
|
1785
|
+
const dx = centerline.deltaX[segmentId];
|
|
1786
|
+
const dy = centerline.deltaY[segmentId];
|
|
1787
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
1788
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
535
1789
|
: 0;
|
|
536
1790
|
const x = ax + dx * amount;
|
|
537
1791
|
const y = ay + dy * amount;
|
|
538
1792
|
const distance = centerline.startDistance[segmentId] +
|
|
539
|
-
|
|
1793
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
540
1794
|
const heading = centerline.heading[segmentId];
|
|
541
1795
|
const normalX = centerline.normalX[segmentId];
|
|
542
1796
|
const normalY = centerline.normalY[segmentId];
|
|
@@ -560,6 +1814,7 @@ function projectIndexedSegment(centerline, segmentId, position) {
|
|
|
560
1814
|
}
|
|
561
1815
|
|
|
562
1816
|
function segmentFromIndex(centerline, segmentId) {
|
|
1817
|
+
if (centerline?.stats) centerline.stats.raySegmentObjectAllocations += 1;
|
|
563
1818
|
return {
|
|
564
1819
|
segmentId,
|
|
565
1820
|
startX: centerline.startX[segmentId],
|
|
@@ -594,12 +1849,22 @@ function seedRayTraceFromNearbyCache(
|
|
|
594
1849
|
if (!originCell) return null;
|
|
595
1850
|
const bucket = rayTraceBucket(vector);
|
|
596
1851
|
const base = rayTraceBaseKey(grid, originCell, radius);
|
|
597
|
-
const
|
|
598
|
-
|
|
599
|
-
|
|
600
|
-
|
|
601
|
-
|
|
602
|
-
|
|
1852
|
+
const minimumSeedDistance = maxDistance * 0.85;
|
|
1853
|
+
let seed = traceCache.get(rayTraceKey(base, bucket));
|
|
1854
|
+
let bestSeedDistance = seed?.maxDistance ?? -Infinity;
|
|
1855
|
+
if (bestSeedDistance < minimumSeedDistance) {
|
|
1856
|
+
seed = null;
|
|
1857
|
+
bestSeedDistance = -Infinity;
|
|
1858
|
+
}
|
|
1859
|
+
const beforeSeed = traceCache.get(rayTraceKey(base, bucket - 1));
|
|
1860
|
+
if ((beforeSeed?.maxDistance ?? -Infinity) >= minimumSeedDistance && beforeSeed.maxDistance > bestSeedDistance) {
|
|
1861
|
+
seed = beforeSeed;
|
|
1862
|
+
bestSeedDistance = beforeSeed.maxDistance;
|
|
1863
|
+
}
|
|
1864
|
+
const afterSeed = traceCache.get(rayTraceKey(base, bucket + 1));
|
|
1865
|
+
if ((afterSeed?.maxDistance ?? -Infinity) >= minimumSeedDistance && afterSeed.maxDistance > bestSeedDistance) {
|
|
1866
|
+
seed = afterSeed;
|
|
1867
|
+
}
|
|
603
1868
|
if (!seed) return { base, bucket };
|
|
604
1869
|
|
|
605
1870
|
for (const segmentId of seed.segmentIds) {
|
|
@@ -621,11 +1886,25 @@ function storeRayTraceCache(index, grid, origin, vector, radius, maxDistance, se
|
|
|
621
1886
|
const bucket = seed?.bucket ?? rayTraceBucket(vector);
|
|
622
1887
|
const base = seed?.base ?? rayTraceBaseKey(grid, originCell, radius);
|
|
623
1888
|
const key = rayTraceKey(base, bucket);
|
|
624
|
-
traceCache.
|
|
625
|
-
|
|
626
|
-
|
|
627
|
-
|
|
628
|
-
|
|
1889
|
+
let entry = traceCache.get(key);
|
|
1890
|
+
if (!entry) {
|
|
1891
|
+
entry = {
|
|
1892
|
+
maxDistance: 0,
|
|
1893
|
+
segmentIds: [],
|
|
1894
|
+
cellIds: [],
|
|
1895
|
+
};
|
|
1896
|
+
traceCache.set(key, entry);
|
|
1897
|
+
if (index.stats) index.stats.rayTraceCacheEntryAllocations += 1;
|
|
1898
|
+
}
|
|
1899
|
+
entry.maxDistance = maxDistance;
|
|
1900
|
+
entry.segmentIds.length = segmentIds.length;
|
|
1901
|
+
for (let index = 0; index < segmentIds.length; index += 1) {
|
|
1902
|
+
entry.segmentIds[index] = segmentIds[index];
|
|
1903
|
+
}
|
|
1904
|
+
entry.cellIds.length = cellIds.length;
|
|
1905
|
+
for (let index = 0; index < cellIds.length; index += 1) {
|
|
1906
|
+
entry.cellIds[index] = cellIds[index];
|
|
1907
|
+
}
|
|
629
1908
|
if (traceCache.size > 96) {
|
|
630
1909
|
const oldestKey = traceCache.keys().next().value;
|
|
631
1910
|
if (oldestKey) traceCache.delete(oldestKey);
|