@inventure71/paddockjs 4.2.0 → 4.3.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 +34 -0
- package/INSTALL_AND_UPDATE.md +23 -4
- package/README.md +31 -2
- package/docs/advanced_ai_pace.md +131 -0
- package/docs/advanced_physics_rebuild.md +166 -0
- package/docs/architecture.md +109 -30
- package/docs/cleanup_coverage.md +284 -0
- package/docs/component_inventory.md +31 -15
- package/docs/composable-layouts.md +2 -0
- package/docs/concepts.md +12 -1
- package/docs/custom_model_controller.md +1 -1
- package/docs/data_contract.md +33 -10
- package/docs/data_model.md +4 -2
- package/docs/efficiency_cleanup_2026-10-13.md +80 -0
- package/docs/grid_walk.md +80 -0
- package/docs/index.md +1 -0
- package/docs/keyboard_driving_fix.md +118 -0
- package/docs/limit_driving_2026-10-14.md +88 -0
- package/docs/maintainability_audit_2026-09-08.md +146 -0
- package/docs/racing_ai_improvements.md +145 -0
- package/docs/rules.md +43 -13
- package/docs/sense_contract.md +4 -1
- package/docs/system_specs.md +36 -11
- package/docs/testing-reliability-guide.md +5 -0
- package/docs/v11_improvements.md +108 -0
- package/package.json +23 -13
- package/src/api/MountedSimulatorComponents.js +101 -0
- package/src/api/PaddockSimulatorController.js +56 -81
- package/src/app/BrowserExpertAdapter.js +0 -6
- package/src/app/BrowserExpertExternalRenderer.js +72 -50
- package/src/app/ExternalTrackSnapshot.js +50 -0
- package/src/app/F1SimulatorApp.js +242 -297
- package/src/app/HostLifecycleEmitter.js +93 -0
- package/src/app/banners/raceDataBannerController.js +9 -4
- package/src/app/camera/cameraConstants.js +1 -1
- package/src/app/camera/cameraController.js +10 -13
- package/src/app/camera/cameraGeometry.js +1 -1
- package/src/app/controls/SimulatorViewControls.js +45 -0
- package/src/app/controls/workbenchBindings.js +64 -0
- package/src/app/controls/workbenchDisclosure.js +41 -0
- package/src/app/domBindings.js +21 -13
- package/src/app/gridWalk/GridWalkController.js +180 -0
- package/src/app/gridWalk/gridWalkCamera.js +88 -0
- package/src/app/gridWalk/gridWalkControls.js +24 -0
- package/src/app/gridWalk/gridWalkCrew.js +246 -0
- package/src/app/gridWalk/gridWalkLabels.js +104 -0
- package/src/app/gridWalk/gridWalkLayout.js +46 -0
- package/src/app/gridWalk/gridWalkNavigation.js +99 -0
- package/src/app/gridWalk/gridWalkProjectBanner.js +43 -0
- package/src/app/gridWalk/gridWalkSession.js +50 -0
- package/src/app/layoutSupport.js +2 -0
- package/src/app/raceOverlayClearanceSupport.js +21 -7
- package/src/app/readouts/carOverviewRenderer.js +9 -4
- package/src/app/readouts/readoutFormatters.js +1 -8
- package/src/app/readouts/telemetryRenderer.js +1 -3
- package/src/app/readouts/timingTowerRenderer.js +48 -50
- package/src/app/rendering/appAssets.js +8 -10
- package/src/app/rendering/carRenderer.js +27 -79
- package/src/app/rendering/displayUtils.js +0 -6
- package/src/app/rendering/expertSensorRenderer.js +1 -1
- package/src/app/rendering/replayGhostRenderer.js +4 -1
- package/src/app/rendering/trackRenderer.js +2 -2
- package/src/app/runtime/frameLoop.js +18 -17
- package/src/app/runtime/pixiStartupLifecycle.js +94 -0
- package/src/app/runtime/runtimeTiming.js +4 -1
- package/src/app/runtime/runtimeVisibility.js +7 -2
- package/src/config/cssValues.js +1 -1
- package/src/config/defaultOptions.js +12 -173
- package/src/config/restartOptions.js +1 -0
- package/src/config/runtimeSeed.js +10 -0
- package/src/config/simulatorDefaults.js +106 -0
- package/src/config/telemetryModules.js +20 -0
- package/src/config/theme/cssTokens.js +85 -0
- package/src/config/theme/themeCss.js +72 -0
- package/src/config/theme/themeInputs.js +155 -0
- package/src/config/theme/themeResolver.js +172 -0
- package/src/config/theme/themeSelectors.js +69 -0
- package/src/config/theme/themeTokens.js +257 -0
- package/src/config/theme/themeUtils.js +15 -0
- package/src/config/themeOptions.js +14 -830
- package/src/config/timingGapMode.js +0 -2
- package/src/config/uiOptions.js +53 -0
- package/src/data/championship.js +16 -158
- package/src/data/championshipDefaults.js +87 -0
- package/src/data/driverData.js +10 -20
- package/src/data/entryIdentity.js +67 -0
- package/src/data/entryPresentation.js +23 -0
- package/src/data/entryTypes.d.ts +160 -0
- package/src/data/index.d.ts +13 -259
- package/src/data/ratingScale.js +14 -0
- package/src/data/teamTypes.d.ts +23 -0
- package/src/data/trackTypes.d.ts +101 -0
- package/src/data/vehicleData.js +12 -23
- package/src/environment/actions.js +3 -2
- package/src/environment/controllerLoop.js +57 -44
- package/src/environment/controllerScheduler.js +30 -0
- package/src/environment/driverObservations.js +260 -0
- package/src/environment/driverReset.js +51 -0
- package/src/environment/episode.js +0 -1
- package/src/environment/evaluation.js +6 -90
- package/src/environment/evaluationTracker.js +100 -0
- package/src/environment/events.js +23 -0
- package/src/environment/index.d.ts +21 -140
- package/src/environment/metrics.js +3 -17
- package/src/environment/observationSchema.js +104 -0
- package/src/environment/observationVector.js +16 -115
- package/src/environment/observations.js +16 -316
- package/src/environment/options.js +5 -24
- package/src/environment/renderFrames.js +21 -0
- package/src/environment/resetOptions.js +41 -0
- package/src/environment/results.js +125 -0
- package/src/environment/rewards.js +10 -16
- package/src/environment/runtime.js +17 -300
- package/src/environment/scenarioOptions.js +90 -0
- package/src/environment/scenarioPlacements.js +70 -0
- package/src/environment/scenarioPresets.js +78 -0
- package/src/environment/scenarios.js +5 -236
- package/src/environment/scratch.js +18 -0
- package/src/environment/sensorOptions.js +20 -0
- package/src/environment/sensors/index.js +2 -556
- package/src/environment/sensors/indexedRayBands.js +0 -18
- package/src/environment/sensors/nearbyCars.js +18 -15
- package/src/environment/sensors/opponentRadar.js +3 -2
- package/src/environment/sensors/pitConnectorProximity.js +20 -0
- package/src/environment/sensors/rayBandTrace.js +1 -37
- package/src/environment/sensors/rayBatch.js +81 -0
- package/src/environment/sensors/rayConfig.js +26 -0
- package/src/environment/sensors/rayGeometry.js +0 -8
- package/src/environment/sensors/rayGuards.js +0 -6
- package/src/environment/sensors/rayOutputs.js +75 -0
- package/src/environment/sensors/rayQuery.js +110 -0
- package/src/environment/sensors/rayScratch.js +146 -0
- package/src/environment/sensors/sensorTargets.js +12 -4
- package/src/environment/sensors/surfaceRays.js +6 -61
- package/src/environment/sensors/trackRays.js +7 -80
- package/src/environment/simulationSetup.js +30 -0
- package/src/environment/specs.js +15 -109
- package/src/index.d.ts +34 -259
- package/src/index.js +28 -6
- package/src/placeholder/index.js +1 -8
- package/src/rendering/displayLifecycle.js +6 -0
- package/src/rendering/proceduralTrackAsset.js +1 -1
- package/src/rendering/renderSnapshot.js +3 -0
- package/src/rendering/track/finishLineRenderer.js +1 -1
- package/src/rendering/track/gridRenderer.js +1 -1
- package/src/rendering/track/offsetStrokeSafety.js +2 -2
- package/src/rendering/track/pitLaneRenderer.js +1 -1
- package/src/rendering/track/trackMaterialRenderer.js +4 -4
- package/src/rendering/track/trackRenderConstants.js +0 -1
- package/src/rendering/track/trackRenderGeometry.js +1 -7
- package/src/simulation/collisionGeometry.js +5 -11
- package/src/simulation/driver/advancedPathControls.js +53 -0
- package/src/simulation/driver/advancedRacePace.js +112 -0
- package/src/simulation/driver/advancedRacingControls.js +61 -0
- package/src/simulation/driver/advancedRacingEnvelope.js +255 -0
- package/src/simulation/driver/advancedRacingSteering.js +76 -0
- package/src/simulation/driver/advancedRacingTiming.js +16 -0
- package/src/simulation/driver/arcadeRacePace.js +83 -0
- package/src/simulation/driver/arcadeRacingControls.js +111 -0
- package/src/simulation/driver/arcadeRacingEnvelope.js +63 -0
- package/src/simulation/driver/driverControlConstants.js +0 -2
- package/src/simulation/driver/driverController.js +14 -3
- package/src/simulation/driver/edgeRecovery.js +0 -1
- package/src/simulation/driver/racingControls.js +10 -406
- package/src/simulation/driver/racingDriverProfile.js +48 -0
- package/src/simulation/driver/racingLinePlan.js +48 -15
- package/src/simulation/driver/rejoinControls.js +58 -111
- package/src/simulation/driver/safetyCarControls.js +12 -0
- package/src/simulation/driver/trafficScan.js +89 -6
- package/src/simulation/pit/pitCarPlacement.js +29 -0
- package/src/simulation/pit/pitFlow.js +8 -30
- package/src/simulation/pit/pitIntent.js +4 -3
- package/src/simulation/pit/pitOccupancy.js +2 -2
- package/src/simulation/pit/pitQueue.js +2 -18
- package/src/simulation/pit/pitRouteMovement.js +1 -36
- package/src/simulation/pit/pitRouting.js +5 -81
- package/src/simulation/pit/pitService.js +3 -32
- package/src/simulation/pit/pitServiceConstants.js +1 -0
- package/src/simulation/pit/pitServiceProgress.js +34 -0
- package/src/simulation/pit/pitSnapshots.js +8 -4
- package/src/simulation/pit/pitState.js +1 -5
- package/src/simulation/race/classification.js +1 -8
- package/src/simulation/race/raceFinish.js +3 -8
- package/src/simulation/race/raceOrder.js +2 -13
- package/src/simulation/race/raceProgress.js +6 -20
- package/src/simulation/race/raceSetup.js +1 -1
- package/src/simulation/race/raceStep.js +1 -1
- package/src/simulation/race/redFlag.js +2 -4
- package/src/simulation/race/safetyCar.js +3 -8
- package/src/simulation/race/stalledDnf.js +2 -4
- package/src/simulation/raceSimulation.js +6 -38
- package/src/simulation/replay/replayGhosts.js +44 -32
- package/src/simulation/rules/collisionSteward.js +0 -14
- package/src/simulation/rules/penaltyActions.js +37 -0
- package/src/simulation/rules/penaltyLedger.js +8 -1
- package/src/simulation/rules/rulesReview.js +1 -7
- package/src/simulation/simMath.js +4 -0
- package/src/simulation/snapshots/raceSnapshots.js +30 -11
- package/src/simulation/timing/drsState.js +8 -0
- package/src/simulation/timing/drsTiming.js +2 -4
- package/src/simulation/timing/lapTelemetry.js +7 -4
- package/src/simulation/timing/sectorPerformance.js +3 -3
- package/src/simulation/timing/timingLines.js +12 -6
- package/src/simulation/track/pitLaneAccess.js +2 -2
- package/src/simulation/track/pitLaneLayout.js +4 -135
- package/src/simulation/track/pitLaneModel.js +70 -0
- package/src/simulation/track/pitLaneState.js +0 -1
- package/src/simulation/track/pitServiceLayout.js +121 -0
- package/src/simulation/track/proceduralTrack.js +77 -0
- package/src/simulation/track/spatialQueries.js +1 -25
- package/src/simulation/track/trackBuilder.js +54 -0
- package/src/simulation/track/trackConstants.js +0 -22
- package/src/simulation/track/trackMath.js +4 -29
- package/src/simulation/track/trackModel.js +4 -241
- package/src/simulation/track/trackModelCache.js +105 -0
- package/src/simulation/track/trackNearestQueries.js +1218 -0
- package/src/simulation/track/trackQueryConstants.js +3 -0
- package/src/simulation/track/trackQueryConstruction.js +368 -0
- package/src/simulation/track/trackQueryIndex.js +12 -1933
- package/src/simulation/track/trackQuerySegments.js +174 -0
- package/src/simulation/track/trackQueryStats.js +12 -0
- package/src/simulation/track/trackRayQueries.js +165 -0
- package/src/simulation/vehicle/advancedContactResponse.js +89 -0
- package/src/simulation/vehicle/advancedTireForces.js +92 -0
- package/src/simulation/vehicle/advancedVehicleModel.js +65 -0
- package/src/simulation/vehicle/advancedVehiclePhysics.js +127 -0
- package/src/simulation/vehicle/contactResolution.js +20 -6
- package/src/simulation/vehicle/mainTrackWheelSurface.js +2 -13
- package/src/simulation/vehicle/pitWheelSurface.js +0 -12
- package/src/simulation/vehicle/runoffResponse.js +2 -4
- package/src/simulation/vehicle/sampledWheelSurface.js +266 -0
- package/src/simulation/vehicle/vehicleGeometry.js +0 -56
- package/src/simulation/vehicle/vehicleKinematics.js +10 -0
- package/src/simulation/vehicle/vehiclePhysics.js +35 -194
- package/src/simulation/vehicle/vehicleSnapshots.js +13 -4
- package/src/simulation/vehicle/vehicleState.js +2 -1
- package/src/simulation/vehicle/wheelSurface.js +13 -432
- package/src/simulation/vehicle/wheelSurfaceCache.js +55 -0
- package/src/simulation/vehicle/wheelSurfaceSummary.js +101 -0
- package/src/styles.css +173 -19
- package/src/ui/bannerTemplates.js +4 -2
- package/src/ui/cameraControlsTemplate.js +4 -2
- package/src/ui/gridWalkTemplate.js +29 -0
- package/src/ui/htmlEscaping.js +8 -0
- package/src/ui/shellTemplate.js +3 -0
- package/src/ui/telemetryDrawerTemplate.js +4 -2
- package/src/ui/telemetryTemplates.js +3 -22
- package/src/ui/templateUtils.js +2 -8
- package/src/ui/viewControlsTemplate.js +21 -0
|
@@ -0,0 +1,1218 @@
|
|
|
1
|
+
import { clamp, wrapDistance } from '../simMath.js';
|
|
2
|
+
import { candidateIdsFromGrid, candidateIdsFromGridBounds, gridCellForPoint } from './trackQueryGrid.js';
|
|
3
|
+
import { ensureQueryScratch, ensureScratchArray, nextScratchEpoch } from './trackQueryScratch.js';
|
|
4
|
+
import { recordFallback, recordStat } from './trackQueryStats.js';
|
|
5
|
+
import { getPrecomputedSegmentNeighborhood, projectIndexedSegment, segmentFromIndex, writeProjectionCandidate } from './trackQuerySegments.js';
|
|
6
|
+
import { finitePoint } from './trackMath.js';
|
|
7
|
+
import { AMBIGUOUS_DISTANCE_EPSILON, HINTED_RUNOFF_WIDE_SEGMENT_RADIUS, LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD } from './trackQueryConstants.js';
|
|
8
|
+
|
|
9
|
+
const GRID_NEIGHBOR_LIMIT = 2;
|
|
10
|
+
|
|
11
|
+
export function queryNearestTrackProjection(track, position, progressHint = null) {
|
|
12
|
+
return queryNearestTrackProjectionInto(track, position, progressHint);
|
|
13
|
+
}
|
|
14
|
+
|
|
15
|
+
export function queryNearestTrackProjectionInto(track, position, progressHint = null, target = null) {
|
|
16
|
+
const index = track?.queryIndex;
|
|
17
|
+
if (!index?.centerline?.segmentCount) {
|
|
18
|
+
recordFallback(index, 'nearestFallbackReasons', 'missing-index');
|
|
19
|
+
return null;
|
|
20
|
+
}
|
|
21
|
+
if (!finitePoint(position)) {
|
|
22
|
+
recordFallback(index, 'nearestFallbackReasons', 'invalid-position');
|
|
23
|
+
return null;
|
|
24
|
+
}
|
|
25
|
+
index.stats.nearestQueries += 1;
|
|
26
|
+
|
|
27
|
+
const { projection, path, reason } = bestProjectionFromIndex(index, position, progressHint, target);
|
|
28
|
+
if (!projection) {
|
|
29
|
+
recordFallback(index, 'nearestFallbackReasons', reason ?? 'unknown');
|
|
30
|
+
return null;
|
|
31
|
+
}
|
|
32
|
+
|
|
33
|
+
recordStat(index, 'nearestPaths', path);
|
|
34
|
+
return projection;
|
|
35
|
+
}
|
|
36
|
+
|
|
37
|
+
export function queryHintedTrackProjection(track, position, progressHint, { radius = 2 } = {}) {
|
|
38
|
+
const index = track?.queryIndex;
|
|
39
|
+
if (!index?.centerline?.segmentCount || !finitePoint(position) || !Number.isFinite(progressHint)) return null;
|
|
40
|
+
const local = bestProjectionFromHintedSegmentNeighborhood(
|
|
41
|
+
index,
|
|
42
|
+
progressHint,
|
|
43
|
+
Math.max(0, Math.floor(radius)),
|
|
44
|
+
position,
|
|
45
|
+
progressHint,
|
|
46
|
+
);
|
|
47
|
+
if (local.projection && local.projection.crossTrackError <= index.bands.gravelEdge) {
|
|
48
|
+
index.stats.hintedSegmentFastPathQueries += 1;
|
|
49
|
+
recordStat(index, 'hintedArcPaths', local.reason === 'tie-resolved' ? 'segment-hint-local-tie-resolved' : 'segment-hint-local');
|
|
50
|
+
return local.projection;
|
|
51
|
+
}
|
|
52
|
+
if (local.projection) {
|
|
53
|
+
index.stats.hintedSegmentWideRadiusQueries += 1;
|
|
54
|
+
const refined = bestProjectionFromHintedSegmentNeighborhood(
|
|
55
|
+
index,
|
|
56
|
+
progressHint,
|
|
57
|
+
HINTED_RUNOFF_WIDE_SEGMENT_RADIUS,
|
|
58
|
+
position,
|
|
59
|
+
progressHint,
|
|
60
|
+
);
|
|
61
|
+
if (refined.projection) {
|
|
62
|
+
if (!projectionMatches(local.projection, refined.projection)) {
|
|
63
|
+
index.stats.hintedSegmentWideRadiusHits += 1;
|
|
64
|
+
}
|
|
65
|
+
recordStat(
|
|
66
|
+
index,
|
|
67
|
+
'hintedArcPaths',
|
|
68
|
+
refined.reason === 'tie-resolved' ? 'segment-hint-runoff-wide-radius-tie-resolved' : 'segment-hint-runoff-wide-radius',
|
|
69
|
+
);
|
|
70
|
+
return refined.projection;
|
|
71
|
+
}
|
|
72
|
+
}
|
|
73
|
+
const { projection, reason } = bestProjectionFromArcBuckets(
|
|
74
|
+
index,
|
|
75
|
+
progressHint,
|
|
76
|
+
Math.max(0, Math.floor(radius)),
|
|
77
|
+
position,
|
|
78
|
+
progressHint,
|
|
79
|
+
);
|
|
80
|
+
if (!projection) return null;
|
|
81
|
+
index.stats.hintedArcQueries += 1;
|
|
82
|
+
recordStat(index, 'hintedArcPaths', reason === 'tie-resolved' ? 'arc-hint-local-tie-resolved' : 'arc-hint-local');
|
|
83
|
+
return projection;
|
|
84
|
+
}
|
|
85
|
+
|
|
86
|
+
export function querySegmentNeighborhoodProjection(
|
|
87
|
+
track,
|
|
88
|
+
position,
|
|
89
|
+
segmentId,
|
|
90
|
+
{ radius = 3, preferredDistance = null, target = null } = {},
|
|
91
|
+
) {
|
|
92
|
+
const index = track?.queryIndex;
|
|
93
|
+
if (!index?.centerline?.segmentCount || !finitePoint(position) || !Number.isInteger(segmentId)) return null;
|
|
94
|
+
const { projection, reason } = bestProjectionFromSegmentNeighborhood(
|
|
95
|
+
index.centerline,
|
|
96
|
+
segmentId,
|
|
97
|
+
Math.max(0, Math.floor(radius)),
|
|
98
|
+
position,
|
|
99
|
+
preferredDistance,
|
|
100
|
+
target,
|
|
101
|
+
);
|
|
102
|
+
if (!projection) return null;
|
|
103
|
+
index.stats.segmentNeighborhoodQueries += 1;
|
|
104
|
+
recordStat(
|
|
105
|
+
index,
|
|
106
|
+
'segmentNeighborhoodPaths',
|
|
107
|
+
reason === 'tie-resolved' ? 'segment-neighborhood-tie-resolved' : 'segment-neighborhood',
|
|
108
|
+
);
|
|
109
|
+
return projection;
|
|
110
|
+
}
|
|
111
|
+
|
|
112
|
+
export function querySegmentNeighborhoodProjections(
|
|
113
|
+
track,
|
|
114
|
+
positions,
|
|
115
|
+
segmentId,
|
|
116
|
+
{ radius = 3, preferredDistance = null, target = null } = {},
|
|
117
|
+
) {
|
|
118
|
+
const index = track?.queryIndex;
|
|
119
|
+
if (!index?.centerline?.segmentCount || !Array.isArray(positions) || !Number.isInteger(segmentId)) return null;
|
|
120
|
+
index.stats.segmentNeighborhoodBatchCalls += 1;
|
|
121
|
+
const projections = target ?? new Array(positions.length);
|
|
122
|
+
const tieResolvedFlags = bestProjectionBatchFromSegmentNeighborhood(
|
|
123
|
+
index,
|
|
124
|
+
positions,
|
|
125
|
+
segmentId,
|
|
126
|
+
Math.max(0, Math.floor(radius)),
|
|
127
|
+
preferredDistance,
|
|
128
|
+
projections,
|
|
129
|
+
);
|
|
130
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
131
|
+
const projection = projections[indexPosition];
|
|
132
|
+
if (!projection) continue;
|
|
133
|
+
index.stats.segmentNeighborhoodQueries += 1;
|
|
134
|
+
recordStat(
|
|
135
|
+
index,
|
|
136
|
+
'segmentNeighborhoodPaths',
|
|
137
|
+
tieResolvedFlags[indexPosition] ? 'segment-neighborhood-tie-resolved' : 'segment-neighborhood',
|
|
138
|
+
);
|
|
139
|
+
}
|
|
140
|
+
return projections;
|
|
141
|
+
}
|
|
142
|
+
|
|
143
|
+
function bestProjectionFromIndex(index, position, progressHint, target = null) {
|
|
144
|
+
if (Number.isFinite(progressHint)) {
|
|
145
|
+
const isolatedHint = bestProjectionFromHintedSegmentNeighborhood(
|
|
146
|
+
index,
|
|
147
|
+
progressHint,
|
|
148
|
+
2,
|
|
149
|
+
position,
|
|
150
|
+
progressHint,
|
|
151
|
+
target,
|
|
152
|
+
);
|
|
153
|
+
if (
|
|
154
|
+
isolatedHint.projection &&
|
|
155
|
+
isolatedHint.projection.crossTrackError <= index.bands.kerbEdge &&
|
|
156
|
+
hintedQueryStaysInLocalCorridor(index, position, progressHint, index.bands.kerbEdge)
|
|
157
|
+
) {
|
|
158
|
+
if (isHintProjectionDistanceSafe(index, isolatedHint.projection)) {
|
|
159
|
+
index.stats.nearestIsolatedHintQueries += 1;
|
|
160
|
+
return {
|
|
161
|
+
projection: isolatedHint.projection,
|
|
162
|
+
path: isolatedHint.reason === 'tie-resolved' ? 'segment-hint-distance-gated-tie-resolved' : 'segment-hint-distance-gated',
|
|
163
|
+
};
|
|
164
|
+
}
|
|
165
|
+
if (isHintProjectionLowDensitySafe(index, position)) {
|
|
166
|
+
index.stats.nearestLowDensityHintQueries += 1;
|
|
167
|
+
return {
|
|
168
|
+
projection: isolatedHint.projection,
|
|
169
|
+
path: isolatedHint.reason === 'tie-resolved' ? 'segment-hint-density-gated-tie-resolved' : 'segment-hint-density-gated',
|
|
170
|
+
};
|
|
171
|
+
}
|
|
172
|
+
}
|
|
173
|
+
const lowDensityExact = bestProjectionFromLowDensityCellNeighborhood(index, position, progressHint, target);
|
|
174
|
+
if (lowDensityExact.projection) {
|
|
175
|
+
index.stats.nearestLowDensityCellExactQueries += 1;
|
|
176
|
+
index.stats.nearestLowDensityCellDirectQueries += 1;
|
|
177
|
+
return {
|
|
178
|
+
projection: lowDensityExact.projection,
|
|
179
|
+
path: lowDensityExact.reason === 'tie-resolved'
|
|
180
|
+
? 'low-density-cell-exact-tie-resolved'
|
|
181
|
+
: 'low-density-cell-exact',
|
|
182
|
+
};
|
|
183
|
+
}
|
|
184
|
+
if (!hintedQueryStaysInLocalCorridor(index, position, progressHint, index.bands.runoffEdge)) {
|
|
185
|
+
const sparseExact = bestProjectionFromCellSearch(index, position, progressHint, null, target);
|
|
186
|
+
if (sparseExact.projection) {
|
|
187
|
+
index.stats.nearestSparseGridExactQueries += 1;
|
|
188
|
+
return {
|
|
189
|
+
projection: sparseExact.projection,
|
|
190
|
+
path: sparseExact.reason === 'tie-resolved'
|
|
191
|
+
? 'sparse-grid-exact-tie-resolved'
|
|
192
|
+
: 'sparse-grid-exact',
|
|
193
|
+
};
|
|
194
|
+
}
|
|
195
|
+
}
|
|
196
|
+
|
|
197
|
+
const hinted = bestProjectionFromArcBuckets(index, progressHint, 2, position, progressHint, target);
|
|
198
|
+
if (hinted.projection) {
|
|
199
|
+
let initialProjection = hinted.projection;
|
|
200
|
+
if (
|
|
201
|
+
hinted.projection.crossTrackError > index.bands.gravelEdge &&
|
|
202
|
+
hinted.projection.crossTrackError <= index.bands.runoffEdge + AMBIGUOUS_DISTANCE_EPSILON
|
|
203
|
+
) {
|
|
204
|
+
// Runoff-adjacent accurate-hint queries often converge inside the local radius-2 segment
|
|
205
|
+
// neighborhood; ring-3 cell lower bounds are enough to prove no outside segment can tie or beat it.
|
|
206
|
+
const ring2NeighborhoodExact = bestProjectionFromRing2Neighborhood(index, position, progressHint, target);
|
|
207
|
+
if (ring2NeighborhoodExact.projection) {
|
|
208
|
+
index.stats.nearestRing2NeighborhoodExactQueries += 1;
|
|
209
|
+
return {
|
|
210
|
+
projection: ring2NeighborhoodExact.projection,
|
|
211
|
+
path: ring2NeighborhoodExact.reason === 'tie-resolved'
|
|
212
|
+
? 'arc-hint-ring2-neighborhood-exact-tie-resolved'
|
|
213
|
+
: 'arc-hint-ring2-neighborhood-exact',
|
|
214
|
+
};
|
|
215
|
+
}
|
|
216
|
+
if (ring2NeighborhoodExact.bestProjection) {
|
|
217
|
+
initialProjection = ring2NeighborhoodExact.bestProjection;
|
|
218
|
+
}
|
|
219
|
+
}
|
|
220
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint, initialProjection);
|
|
221
|
+
if (exact.projection) {
|
|
222
|
+
const path = projectionMatches(hinted.projection, exact.projection)
|
|
223
|
+
? (hinted.reason === 'tie-resolved' ? 'arc-hint-tie-resolved' : 'arc-hint')
|
|
224
|
+
: 'arc-hint-refined';
|
|
225
|
+
return {
|
|
226
|
+
projection: exact.projection,
|
|
227
|
+
path: exact.reason === 'tie-resolved' && !path.endsWith('tie-resolved') ? `${path}-tie-resolved` : path,
|
|
228
|
+
};
|
|
229
|
+
}
|
|
230
|
+
}
|
|
231
|
+
}
|
|
232
|
+
|
|
233
|
+
let local = { projection: null, path: null };
|
|
234
|
+
const gridCandidateIds = candidateIdsFromGrid(index, index.grid, position, GRID_NEIGHBOR_LIMIT);
|
|
235
|
+
const grid = bestProjectionFromCandidates(index.centerline, gridCandidateIds, position, progressHint, target);
|
|
236
|
+
if (grid.projection) {
|
|
237
|
+
local = chooseBetterProjectionResult(local, grid, 'spatial-grid', progressHint);
|
|
238
|
+
if (grid.projection.distanceSquared <= index.nearestGridExpansion * index.nearestGridExpansion) {
|
|
239
|
+
return local;
|
|
240
|
+
}
|
|
241
|
+
}
|
|
242
|
+
|
|
243
|
+
if (local.projection) {
|
|
244
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint, local.projection);
|
|
245
|
+
if (exact.projection) {
|
|
246
|
+
const path = projectionMatches(local.projection, exact.projection)
|
|
247
|
+
? local.path
|
|
248
|
+
: `${local.path}-refined`;
|
|
249
|
+
return {
|
|
250
|
+
projection: exact.projection,
|
|
251
|
+
path: exact.reason === 'tie-resolved' && !path.endsWith('tie-resolved') ? `${path}-tie-resolved` : path,
|
|
252
|
+
};
|
|
253
|
+
}
|
|
254
|
+
|
|
255
|
+
return {
|
|
256
|
+
projection: local.projection,
|
|
257
|
+
path: local.path,
|
|
258
|
+
};
|
|
259
|
+
}
|
|
260
|
+
|
|
261
|
+
const exact = bestProjectionFromCellSearch(index, position, progressHint);
|
|
262
|
+
const global = exact.projection ? exact : bestProjectionFromAllSegments(index.centerline, position, progressHint, target);
|
|
263
|
+
return global.projection
|
|
264
|
+
? {
|
|
265
|
+
projection: global.projection,
|
|
266
|
+
path: exact.projection
|
|
267
|
+
? (global.reason === 'tie-resolved' ? 'exact-grid-tie-resolved' : 'exact-grid')
|
|
268
|
+
: (global.reason === 'tie-resolved' ? 'global-index-tie-resolved' : 'global-index'),
|
|
269
|
+
}
|
|
270
|
+
: { projection: null, reason: `global-index-${global.reason}` };
|
|
271
|
+
}
|
|
272
|
+
|
|
273
|
+
function chooseBetterProjectionResult(current, candidate, source, preferredDistance) {
|
|
274
|
+
if (!candidate.projection) return current;
|
|
275
|
+
const path = candidate.reason === 'tie-resolved' ? `${source}-tie-resolved` : source;
|
|
276
|
+
if (!current.projection) return { projection: candidate.projection, path };
|
|
277
|
+
if (candidate.projection.distanceSquared < current.projection.distanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
278
|
+
return { projection: candidate.projection, path };
|
|
279
|
+
}
|
|
280
|
+
if (Math.abs(candidate.projection.distanceSquared - current.projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
281
|
+
const candidateTie = projectionTieScore(candidate.projection, preferredDistance);
|
|
282
|
+
const currentTie = projectionTieScore(current.projection, preferredDistance);
|
|
283
|
+
if (projectionTieBeats(candidate.projection, candidateTie, current.projection, currentTie)) {
|
|
284
|
+
return { projection: candidate.projection, path: `${path}-tie-resolved` };
|
|
285
|
+
}
|
|
286
|
+
}
|
|
287
|
+
return current;
|
|
288
|
+
}
|
|
289
|
+
|
|
290
|
+
function bestProjectionFromCellSearch(index, position, preferredDistance, initialProjection = null, target = null) {
|
|
291
|
+
const grid = index.segmentGrid ?? index.grid;
|
|
292
|
+
const center = gridCellForPoint(grid, position, true);
|
|
293
|
+
const startCellIndex = center.row * grid.columns + center.column;
|
|
294
|
+
const startRadius = index.nearestOccupiedCellRadiusByCell?.[startCellIndex] ?? 0;
|
|
295
|
+
if (startRadius > 0) {
|
|
296
|
+
index.stats.nearestEmptyCellSkippedRings += startRadius;
|
|
297
|
+
}
|
|
298
|
+
|
|
299
|
+
const scratch = ensureQueryScratch(index);
|
|
300
|
+
const segmentMarks = ensureScratchArray(scratch, 'exactSegmentMarks', index.centerline.segmentCount);
|
|
301
|
+
const cellMarks = ensureScratchArray(scratch, 'exactCellMarks', grid.columns * grid.rows);
|
|
302
|
+
const segmentEpoch = nextScratchEpoch(scratch, 'exactSegmentEpoch', segmentMarks);
|
|
303
|
+
const cellEpoch = nextScratchEpoch(scratch, 'exactCellEpoch', cellMarks);
|
|
304
|
+
const maxRadius = Math.max(grid.columns, grid.rows);
|
|
305
|
+
let bestSegmentId = initialProjection?.segmentId ?? null;
|
|
306
|
+
let bestX = initialProjection?.x ?? 0;
|
|
307
|
+
let bestY = initialProjection?.y ?? 0;
|
|
308
|
+
let bestDistance = initialProjection?.distance ?? 0;
|
|
309
|
+
let bestHeading = initialProjection?.heading ?? 0;
|
|
310
|
+
let bestNormalX = initialProjection?.normalX ?? 0;
|
|
311
|
+
let bestNormalY = initialProjection?.normalY ?? 0;
|
|
312
|
+
let bestCurvature = initialProjection?.curvature ?? 0;
|
|
313
|
+
let bestSignedOffset = initialProjection?.signedOffset ?? 0;
|
|
314
|
+
let bestDistanceSquared = initialProjection?.distanceSquared ?? Infinity;
|
|
315
|
+
let bestTieScore = initialProjection ? projectionTieScore(initialProjection, preferredDistance) : Infinity;
|
|
316
|
+
let secondBestDistance = Infinity;
|
|
317
|
+
let tieResolved = false;
|
|
318
|
+
if (Number.isInteger(bestSegmentId)) {
|
|
319
|
+
segmentMarks[bestSegmentId] = segmentEpoch;
|
|
320
|
+
}
|
|
321
|
+
|
|
322
|
+
for (let radius = startRadius; radius <= maxRadius; radius += 1) {
|
|
323
|
+
let ringCanImprove = bestSegmentId == null;
|
|
324
|
+
forEachRingCell(grid, center, radius, (cellIndex, row, column) => {
|
|
325
|
+
if (cellMarks[cellIndex] === cellEpoch) return;
|
|
326
|
+
cellMarks[cellIndex] = cellEpoch;
|
|
327
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
328
|
+
if (bestSegmentId != null && lowerBound > bestDistanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
329
|
+
return;
|
|
330
|
+
}
|
|
331
|
+
ringCanImprove = true;
|
|
332
|
+
const cell = grid.cells[cellIndex];
|
|
333
|
+
if (!cell) return;
|
|
334
|
+
for (const segmentId of cell) {
|
|
335
|
+
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
336
|
+
segmentMarks[segmentId] = segmentEpoch;
|
|
337
|
+
const ax = index.centerline.startX[segmentId];
|
|
338
|
+
const ay = index.centerline.startY[segmentId];
|
|
339
|
+
const dx = index.centerline.deltaX[segmentId];
|
|
340
|
+
const dy = index.centerline.deltaY[segmentId];
|
|
341
|
+
const amount = index.centerline.lengthSquared[segmentId] > 0
|
|
342
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / index.centerline.lengthSquared[segmentId], 0, 1)
|
|
343
|
+
: 0;
|
|
344
|
+
const x = ax + dx * amount;
|
|
345
|
+
const y = ay + dy * amount;
|
|
346
|
+
const distance = index.centerline.startDistance[segmentId] +
|
|
347
|
+
index.centerline.distanceSpan[segmentId] * amount;
|
|
348
|
+
const normalX = index.centerline.normalX[segmentId];
|
|
349
|
+
const normalY = index.centerline.normalY[segmentId];
|
|
350
|
+
const px = position.x - x;
|
|
351
|
+
const py = position.y - y;
|
|
352
|
+
const signedOffset = px * normalX + py * normalY;
|
|
353
|
+
const distanceSquared = px * px + py * py;
|
|
354
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
355
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
356
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
357
|
+
bestSegmentId = segmentId;
|
|
358
|
+
bestX = x;
|
|
359
|
+
bestY = y;
|
|
360
|
+
bestDistance = distance;
|
|
361
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
362
|
+
bestNormalX = normalX;
|
|
363
|
+
bestNormalY = normalY;
|
|
364
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
365
|
+
bestSignedOffset = signedOffset;
|
|
366
|
+
bestDistanceSquared = distanceSquared;
|
|
367
|
+
bestTieScore = tieScore;
|
|
368
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
369
|
+
tieResolved = true;
|
|
370
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
371
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
372
|
+
bestSegmentId = segmentId;
|
|
373
|
+
bestX = x;
|
|
374
|
+
bestY = y;
|
|
375
|
+
bestDistance = distance;
|
|
376
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
377
|
+
bestNormalX = normalX;
|
|
378
|
+
bestNormalY = normalY;
|
|
379
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
380
|
+
bestSignedOffset = signedOffset;
|
|
381
|
+
bestDistanceSquared = distanceSquared;
|
|
382
|
+
bestTieScore = tieScore;
|
|
383
|
+
}
|
|
384
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
385
|
+
secondBestDistance = distanceSquared;
|
|
386
|
+
}
|
|
387
|
+
}
|
|
388
|
+
});
|
|
389
|
+
if (bestSegmentId != null && !ringCanImprove) break;
|
|
390
|
+
}
|
|
391
|
+
|
|
392
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-candidates' };
|
|
393
|
+
const projection = initialProjection ?? target ?? {};
|
|
394
|
+
writeProjectionCandidate(projection, {
|
|
395
|
+
segmentId: bestSegmentId,
|
|
396
|
+
x: bestX,
|
|
397
|
+
y: bestY,
|
|
398
|
+
distance: bestDistance,
|
|
399
|
+
heading: bestHeading,
|
|
400
|
+
normalX: bestNormalX,
|
|
401
|
+
normalY: bestNormalY,
|
|
402
|
+
curvature: bestCurvature,
|
|
403
|
+
signedOffset: bestSignedOffset,
|
|
404
|
+
distanceSquared: bestDistanceSquared,
|
|
405
|
+
});
|
|
406
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
407
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
408
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
409
|
+
}
|
|
410
|
+
|
|
411
|
+
function projectionMatches(first, second) {
|
|
412
|
+
return first.segmentId === second.segmentId &&
|
|
413
|
+
Math.abs(first.distanceSquared - second.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
414
|
+
}
|
|
415
|
+
|
|
416
|
+
function forEachRingCell(grid, center, radius, visitor) {
|
|
417
|
+
const minRow = Math.max(0, center.row - radius);
|
|
418
|
+
const maxRow = Math.min(grid.rows - 1, center.row + radius);
|
|
419
|
+
const minColumn = Math.max(0, center.column - radius);
|
|
420
|
+
const maxColumn = Math.min(grid.columns - 1, center.column + radius);
|
|
421
|
+
for (let row = minRow; row <= maxRow; row += 1) {
|
|
422
|
+
for (let column = minColumn; column <= maxColumn; column += 1) {
|
|
423
|
+
if (
|
|
424
|
+
radius > 0 &&
|
|
425
|
+
row !== minRow &&
|
|
426
|
+
row !== maxRow &&
|
|
427
|
+
column !== minColumn &&
|
|
428
|
+
column !== maxColumn
|
|
429
|
+
) continue;
|
|
430
|
+
visitor(row * grid.columns + column, row, column);
|
|
431
|
+
}
|
|
432
|
+
}
|
|
433
|
+
}
|
|
434
|
+
|
|
435
|
+
function cellDistanceSquared(grid, row, column, position) {
|
|
436
|
+
const minX = grid.bounds.minX + column * grid.cellSize;
|
|
437
|
+
const minY = grid.bounds.minY + row * grid.cellSize;
|
|
438
|
+
const maxX = Math.min(grid.bounds.maxX, minX + grid.cellSize);
|
|
439
|
+
const maxY = Math.min(grid.bounds.maxY, minY + grid.cellSize);
|
|
440
|
+
const dx = position.x < minX ? minX - position.x : position.x > maxX ? position.x - maxX : 0;
|
|
441
|
+
const dy = position.y < minY ? minY - position.y : position.y > maxY ? position.y - maxY : 0;
|
|
442
|
+
return dx * dx + dy * dy;
|
|
443
|
+
}
|
|
444
|
+
|
|
445
|
+
function bestProjectionFromCandidates(centerline, candidateIds, position, preferredDistance = null, target = null) {
|
|
446
|
+
if (!candidateIds.length) return { projection: null, reason: 'no-candidates' };
|
|
447
|
+
let bestSegmentId = null;
|
|
448
|
+
let bestX = 0;
|
|
449
|
+
let bestY = 0;
|
|
450
|
+
let bestDistance = 0;
|
|
451
|
+
let bestHeading = 0;
|
|
452
|
+
let bestNormalX = 0;
|
|
453
|
+
let bestNormalY = 0;
|
|
454
|
+
let bestCurvature = 0;
|
|
455
|
+
let bestSignedOffset = 0;
|
|
456
|
+
let bestDistanceSquared = Infinity;
|
|
457
|
+
let bestTieScore = Infinity;
|
|
458
|
+
let secondBestDistance = Infinity;
|
|
459
|
+
let tieResolved = false;
|
|
460
|
+
for (const segmentId of candidateIds) {
|
|
461
|
+
const ax = centerline.startX[segmentId];
|
|
462
|
+
const ay = centerline.startY[segmentId];
|
|
463
|
+
const dx = centerline.deltaX[segmentId];
|
|
464
|
+
const dy = centerline.deltaY[segmentId];
|
|
465
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
466
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
467
|
+
: 0;
|
|
468
|
+
const x = ax + dx * amount;
|
|
469
|
+
const y = ay + dy * amount;
|
|
470
|
+
const distance = centerline.startDistance[segmentId] +
|
|
471
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
472
|
+
const normalX = centerline.normalX[segmentId];
|
|
473
|
+
const normalY = centerline.normalY[segmentId];
|
|
474
|
+
const px = position.x - x;
|
|
475
|
+
const py = position.y - y;
|
|
476
|
+
const signedOffset = px * normalX + py * normalY;
|
|
477
|
+
const distanceSquared = px * px + py * py;
|
|
478
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
479
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
480
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
481
|
+
bestSegmentId = segmentId;
|
|
482
|
+
bestX = x;
|
|
483
|
+
bestY = y;
|
|
484
|
+
bestDistance = distance;
|
|
485
|
+
bestHeading = centerline.heading[segmentId];
|
|
486
|
+
bestNormalX = normalX;
|
|
487
|
+
bestNormalY = normalY;
|
|
488
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
489
|
+
bestSignedOffset = signedOffset;
|
|
490
|
+
bestDistanceSquared = distanceSquared;
|
|
491
|
+
bestTieScore = tieScore;
|
|
492
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
493
|
+
tieResolved = true;
|
|
494
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
495
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
496
|
+
bestSegmentId = segmentId;
|
|
497
|
+
bestX = x;
|
|
498
|
+
bestY = y;
|
|
499
|
+
bestDistance = distance;
|
|
500
|
+
bestHeading = centerline.heading[segmentId];
|
|
501
|
+
bestNormalX = normalX;
|
|
502
|
+
bestNormalY = normalY;
|
|
503
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
504
|
+
bestSignedOffset = signedOffset;
|
|
505
|
+
bestDistanceSquared = distanceSquared;
|
|
506
|
+
bestTieScore = tieScore;
|
|
507
|
+
}
|
|
508
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
509
|
+
secondBestDistance = distanceSquared;
|
|
510
|
+
}
|
|
511
|
+
}
|
|
512
|
+
|
|
513
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
514
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
515
|
+
segmentId: bestSegmentId,
|
|
516
|
+
x: bestX,
|
|
517
|
+
y: bestY,
|
|
518
|
+
distance: bestDistance,
|
|
519
|
+
heading: bestHeading,
|
|
520
|
+
normalX: bestNormalX,
|
|
521
|
+
normalY: bestNormalY,
|
|
522
|
+
curvature: bestCurvature,
|
|
523
|
+
signedOffset: bestSignedOffset,
|
|
524
|
+
distanceSquared: bestDistanceSquared,
|
|
525
|
+
});
|
|
526
|
+
|
|
527
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
528
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
529
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
530
|
+
}
|
|
531
|
+
|
|
532
|
+
function bestProjectionFromAllSegments(centerline, position, preferredDistance = null, target = null) {
|
|
533
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
534
|
+
if (segmentCount <= 0) return { projection: null, reason: 'no-candidates' };
|
|
535
|
+
let bestSegmentId = null;
|
|
536
|
+
let bestX = 0;
|
|
537
|
+
let bestY = 0;
|
|
538
|
+
let bestDistance = 0;
|
|
539
|
+
let bestHeading = 0;
|
|
540
|
+
let bestNormalX = 0;
|
|
541
|
+
let bestNormalY = 0;
|
|
542
|
+
let bestCurvature = 0;
|
|
543
|
+
let bestSignedOffset = 0;
|
|
544
|
+
let bestDistanceSquared = Infinity;
|
|
545
|
+
let bestTieScore = Infinity;
|
|
546
|
+
let secondBestDistance = Infinity;
|
|
547
|
+
let tieResolved = false;
|
|
548
|
+
|
|
549
|
+
for (let segmentId = 0; segmentId < segmentCount; segmentId += 1) {
|
|
550
|
+
const ax = centerline.startX[segmentId];
|
|
551
|
+
const ay = centerline.startY[segmentId];
|
|
552
|
+
const dx = centerline.deltaX[segmentId];
|
|
553
|
+
const dy = centerline.deltaY[segmentId];
|
|
554
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
555
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
556
|
+
: 0;
|
|
557
|
+
const x = ax + dx * amount;
|
|
558
|
+
const y = ay + dy * amount;
|
|
559
|
+
const distance = centerline.startDistance[segmentId] +
|
|
560
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
561
|
+
const normalX = centerline.normalX[segmentId];
|
|
562
|
+
const normalY = centerline.normalY[segmentId];
|
|
563
|
+
const px = position.x - x;
|
|
564
|
+
const py = position.y - y;
|
|
565
|
+
const signedOffset = px * normalX + py * normalY;
|
|
566
|
+
const distanceSquared = px * px + py * py;
|
|
567
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
568
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
569
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
570
|
+
bestSegmentId = segmentId;
|
|
571
|
+
bestX = x;
|
|
572
|
+
bestY = y;
|
|
573
|
+
bestDistance = distance;
|
|
574
|
+
bestHeading = centerline.heading[segmentId];
|
|
575
|
+
bestNormalX = normalX;
|
|
576
|
+
bestNormalY = normalY;
|
|
577
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
578
|
+
bestSignedOffset = signedOffset;
|
|
579
|
+
bestDistanceSquared = distanceSquared;
|
|
580
|
+
bestTieScore = tieScore;
|
|
581
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
582
|
+
tieResolved = true;
|
|
583
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
584
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
585
|
+
bestSegmentId = segmentId;
|
|
586
|
+
bestX = x;
|
|
587
|
+
bestY = y;
|
|
588
|
+
bestDistance = distance;
|
|
589
|
+
bestHeading = centerline.heading[segmentId];
|
|
590
|
+
bestNormalX = normalX;
|
|
591
|
+
bestNormalY = normalY;
|
|
592
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
593
|
+
bestSignedOffset = signedOffset;
|
|
594
|
+
bestDistanceSquared = distanceSquared;
|
|
595
|
+
bestTieScore = tieScore;
|
|
596
|
+
}
|
|
597
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
598
|
+
secondBestDistance = distanceSquared;
|
|
599
|
+
}
|
|
600
|
+
}
|
|
601
|
+
|
|
602
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
603
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
604
|
+
segmentId: bestSegmentId,
|
|
605
|
+
x: bestX,
|
|
606
|
+
y: bestY,
|
|
607
|
+
distance: bestDistance,
|
|
608
|
+
heading: bestHeading,
|
|
609
|
+
normalX: bestNormalX,
|
|
610
|
+
normalY: bestNormalY,
|
|
611
|
+
curvature: bestCurvature,
|
|
612
|
+
signedOffset: bestSignedOffset,
|
|
613
|
+
distanceSquared: bestDistanceSquared,
|
|
614
|
+
});
|
|
615
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
616
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
617
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
618
|
+
}
|
|
619
|
+
|
|
620
|
+
function bestProjectionFromSegmentNeighborhood(
|
|
621
|
+
centerline,
|
|
622
|
+
centerSegmentId,
|
|
623
|
+
radius,
|
|
624
|
+
position,
|
|
625
|
+
preferredDistance = null,
|
|
626
|
+
target = null,
|
|
627
|
+
) {
|
|
628
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
629
|
+
if (segmentCount <= 0) return { projection: null, reason: 'no-candidates' };
|
|
630
|
+
let bestSegmentId = null;
|
|
631
|
+
let bestX = 0;
|
|
632
|
+
let bestY = 0;
|
|
633
|
+
let bestDistance = 0;
|
|
634
|
+
let bestHeading = 0;
|
|
635
|
+
let bestNormalX = 0;
|
|
636
|
+
let bestNormalY = 0;
|
|
637
|
+
let bestCurvature = 0;
|
|
638
|
+
let bestSignedOffset = 0;
|
|
639
|
+
let bestDistanceSquared = Infinity;
|
|
640
|
+
let bestTieScore = Infinity;
|
|
641
|
+
let secondBestDistance = Infinity;
|
|
642
|
+
let tieResolved = false;
|
|
643
|
+
|
|
644
|
+
const precomputedNeighborhood = getPrecomputedSegmentNeighborhood(centerline, centerSegmentId, radius);
|
|
645
|
+
const segmentIds = precomputedNeighborhood ?? null;
|
|
646
|
+
if (centerline?.stats && segmentIds) centerline.stats.precomputedSegmentNeighborhoodHits += 1;
|
|
647
|
+
const segmentIdCount = segmentIds?.length ?? (radius * 2 + 1);
|
|
648
|
+
for (let index = 0; index < segmentIdCount; index += 1) {
|
|
649
|
+
const segmentId = segmentIds
|
|
650
|
+
? segmentIds[index]
|
|
651
|
+
: ((centerSegmentId + (index - radius)) % segmentCount + segmentCount) % segmentCount;
|
|
652
|
+
const ax = centerline.startX[segmentId];
|
|
653
|
+
const ay = centerline.startY[segmentId];
|
|
654
|
+
const dx = centerline.deltaX[segmentId];
|
|
655
|
+
const dy = centerline.deltaY[segmentId];
|
|
656
|
+
const amount = centerline.lengthSquared[segmentId] > 0
|
|
657
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / centerline.lengthSquared[segmentId], 0, 1)
|
|
658
|
+
: 0;
|
|
659
|
+
const x = ax + dx * amount;
|
|
660
|
+
const y = ay + dy * amount;
|
|
661
|
+
const distance = centerline.startDistance[segmentId] +
|
|
662
|
+
centerline.distanceSpan[segmentId] * amount;
|
|
663
|
+
const normalX = centerline.normalX[segmentId];
|
|
664
|
+
const normalY = centerline.normalY[segmentId];
|
|
665
|
+
const px = position.x - x;
|
|
666
|
+
const py = position.y - y;
|
|
667
|
+
const signedOffset = px * normalX + py * normalY;
|
|
668
|
+
const distanceSquared = px * px + py * py;
|
|
669
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
670
|
+
|
|
671
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
672
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
673
|
+
bestSegmentId = segmentId;
|
|
674
|
+
bestX = x;
|
|
675
|
+
bestY = y;
|
|
676
|
+
bestDistance = distance;
|
|
677
|
+
bestHeading = centerline.heading[segmentId];
|
|
678
|
+
bestNormalX = normalX;
|
|
679
|
+
bestNormalY = normalY;
|
|
680
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
681
|
+
bestSignedOffset = signedOffset;
|
|
682
|
+
bestDistanceSquared = distanceSquared;
|
|
683
|
+
bestTieScore = tieScore;
|
|
684
|
+
continue;
|
|
685
|
+
}
|
|
686
|
+
|
|
687
|
+
if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
688
|
+
tieResolved = true;
|
|
689
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
690
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
691
|
+
bestSegmentId = segmentId;
|
|
692
|
+
bestX = x;
|
|
693
|
+
bestY = y;
|
|
694
|
+
bestDistance = distance;
|
|
695
|
+
bestHeading = centerline.heading[segmentId];
|
|
696
|
+
bestNormalX = normalX;
|
|
697
|
+
bestNormalY = normalY;
|
|
698
|
+
bestCurvature = centerline.curvature[segmentId];
|
|
699
|
+
bestSignedOffset = signedOffset;
|
|
700
|
+
bestDistanceSquared = distanceSquared;
|
|
701
|
+
bestTieScore = tieScore;
|
|
702
|
+
}
|
|
703
|
+
continue;
|
|
704
|
+
}
|
|
705
|
+
|
|
706
|
+
if (distanceSquared < secondBestDistance) {
|
|
707
|
+
secondBestDistance = distanceSquared;
|
|
708
|
+
}
|
|
709
|
+
}
|
|
710
|
+
|
|
711
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
712
|
+
const projection = target ?? {};
|
|
713
|
+
projection.segmentId = bestSegmentId;
|
|
714
|
+
projection.x = bestX;
|
|
715
|
+
projection.y = bestY;
|
|
716
|
+
projection.distance = bestDistance;
|
|
717
|
+
projection.heading = bestHeading;
|
|
718
|
+
projection.normalX = bestNormalX;
|
|
719
|
+
projection.normalY = bestNormalY;
|
|
720
|
+
projection.curvature = bestCurvature;
|
|
721
|
+
projection.signedOffset = bestSignedOffset;
|
|
722
|
+
projection.crossTrackError = Math.abs(bestSignedOffset);
|
|
723
|
+
projection.distanceSquared = bestDistanceSquared;
|
|
724
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
725
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
726
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
727
|
+
}
|
|
728
|
+
|
|
729
|
+
function bestProjectionBatchFromSegmentNeighborhood(
|
|
730
|
+
index,
|
|
731
|
+
positions,
|
|
732
|
+
centerSegmentId,
|
|
733
|
+
radius,
|
|
734
|
+
preferredDistance = null,
|
|
735
|
+
target = null,
|
|
736
|
+
) {
|
|
737
|
+
const centerline = index.centerline;
|
|
738
|
+
const projections = target ?? new Array(positions.length);
|
|
739
|
+
projections.length = positions.length;
|
|
740
|
+
const scratch = ensureQueryScratch(index);
|
|
741
|
+
const tieResolvedFlags = ensureUint8ScratchArray(scratch, 'segmentNeighborhoodTieResolved', positions.length);
|
|
742
|
+
const validFlags = ensureUint8ScratchArray(scratch, 'segmentNeighborhoodValid', positions.length);
|
|
743
|
+
const bestTieScores = ensureFloat64ScratchArray(scratch, 'segmentNeighborhoodBestTieScores', positions.length);
|
|
744
|
+
const secondBestDistances = ensureFloat64ScratchArray(scratch, 'segmentNeighborhoodSecondBestDistances', positions.length);
|
|
745
|
+
|
|
746
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
747
|
+
const position = positions[indexPosition];
|
|
748
|
+
if (!finitePoint(position)) {
|
|
749
|
+
projections[indexPosition] = null;
|
|
750
|
+
validFlags[indexPosition] = 0;
|
|
751
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
752
|
+
bestTieScores[indexPosition] = Infinity;
|
|
753
|
+
secondBestDistances[indexPosition] = Infinity;
|
|
754
|
+
continue;
|
|
755
|
+
}
|
|
756
|
+
const projection = projections[indexPosition] ?? {};
|
|
757
|
+
projections[indexPosition] = projection;
|
|
758
|
+
projection.segmentId = -1;
|
|
759
|
+
projection.distanceSquared = Infinity;
|
|
760
|
+
validFlags[indexPosition] = 1;
|
|
761
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
762
|
+
bestTieScores[indexPosition] = Infinity;
|
|
763
|
+
secondBestDistances[indexPosition] = Infinity;
|
|
764
|
+
}
|
|
765
|
+
|
|
766
|
+
const segmentCount = centerline.segmentCount;
|
|
767
|
+
const precomputedNeighborhood = getPrecomputedSegmentNeighborhood(centerline, centerSegmentId, radius);
|
|
768
|
+
if (centerline?.stats && precomputedNeighborhood) centerline.stats.precomputedSegmentNeighborhoodHits += positions.length;
|
|
769
|
+
const segmentIds = precomputedNeighborhood ?? null;
|
|
770
|
+
const segmentIdCount = segmentIds?.length ?? (radius * 2 + 1);
|
|
771
|
+
for (let index = 0; index < segmentIdCount; index += 1) {
|
|
772
|
+
const segmentId = segmentIds
|
|
773
|
+
? segmentIds[index]
|
|
774
|
+
: ((centerSegmentId + (index - radius)) % segmentCount + segmentCount) % segmentCount;
|
|
775
|
+
const ax = centerline.startX[segmentId];
|
|
776
|
+
const ay = centerline.startY[segmentId];
|
|
777
|
+
const dx = centerline.deltaX[segmentId];
|
|
778
|
+
const dy = centerline.deltaY[segmentId];
|
|
779
|
+
const lengthSquared = centerline.lengthSquared[segmentId];
|
|
780
|
+
const distanceBase = centerline.startDistance[segmentId];
|
|
781
|
+
const distanceSpan = centerline.distanceSpan[segmentId];
|
|
782
|
+
const heading = centerline.heading[segmentId];
|
|
783
|
+
const normalX = centerline.normalX[segmentId];
|
|
784
|
+
const normalY = centerline.normalY[segmentId];
|
|
785
|
+
const curvature = centerline.curvature[segmentId];
|
|
786
|
+
|
|
787
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
788
|
+
if (!validFlags[indexPosition]) continue;
|
|
789
|
+
const position = positions[indexPosition];
|
|
790
|
+
const amount = lengthSquared > 0
|
|
791
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / lengthSquared, 0, 1)
|
|
792
|
+
: 0;
|
|
793
|
+
const x = ax + dx * amount;
|
|
794
|
+
const y = ay + dy * amount;
|
|
795
|
+
const distance = distanceBase + distanceSpan * amount;
|
|
796
|
+
const px = position.x - x;
|
|
797
|
+
const py = position.y - y;
|
|
798
|
+
const signedOffset = px * normalX + py * normalY;
|
|
799
|
+
const distanceSquared = px * px + py * py;
|
|
800
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
801
|
+
const projection = projections[indexPosition];
|
|
802
|
+
|
|
803
|
+
if (projection.segmentId < 0 || distanceSquared < projection.distanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
804
|
+
secondBestDistances[indexPosition] = projection.segmentId < 0 ? Infinity : projection.distanceSquared;
|
|
805
|
+
writeProjectionCandidate(projection, {
|
|
806
|
+
segmentId,
|
|
807
|
+
x,
|
|
808
|
+
y,
|
|
809
|
+
distance,
|
|
810
|
+
heading,
|
|
811
|
+
normalX,
|
|
812
|
+
normalY,
|
|
813
|
+
curvature,
|
|
814
|
+
signedOffset,
|
|
815
|
+
distanceSquared,
|
|
816
|
+
});
|
|
817
|
+
bestTieScores[indexPosition] = tieScore;
|
|
818
|
+
continue;
|
|
819
|
+
}
|
|
820
|
+
|
|
821
|
+
if (Math.abs(distanceSquared - projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
822
|
+
tieResolvedFlags[indexPosition] = 1;
|
|
823
|
+
secondBestDistances[indexPosition] = Math.min(secondBestDistances[indexPosition], distanceSquared);
|
|
824
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, projection.segmentId, bestTieScores[indexPosition])) {
|
|
825
|
+
writeProjectionCandidate(projection, {
|
|
826
|
+
segmentId,
|
|
827
|
+
x,
|
|
828
|
+
y,
|
|
829
|
+
distance,
|
|
830
|
+
heading,
|
|
831
|
+
normalX,
|
|
832
|
+
normalY,
|
|
833
|
+
curvature,
|
|
834
|
+
signedOffset,
|
|
835
|
+
distanceSquared,
|
|
836
|
+
});
|
|
837
|
+
bestTieScores[indexPosition] = tieScore;
|
|
838
|
+
}
|
|
839
|
+
continue;
|
|
840
|
+
}
|
|
841
|
+
|
|
842
|
+
if (distanceSquared < secondBestDistances[indexPosition]) {
|
|
843
|
+
secondBestDistances[indexPosition] = distanceSquared;
|
|
844
|
+
}
|
|
845
|
+
}
|
|
846
|
+
}
|
|
847
|
+
|
|
848
|
+
for (let indexPosition = 0; indexPosition < positions.length; indexPosition += 1) {
|
|
849
|
+
if (!validFlags[indexPosition]) continue;
|
|
850
|
+
const projection = projections[indexPosition];
|
|
851
|
+
if (projection.segmentId < 0) {
|
|
852
|
+
projections[indexPosition] = null;
|
|
853
|
+
tieResolvedFlags[indexPosition] = 0;
|
|
854
|
+
continue;
|
|
855
|
+
}
|
|
856
|
+
const ambiguous = Number.isFinite(secondBestDistances[indexPosition]) &&
|
|
857
|
+
Math.abs(secondBestDistances[indexPosition] - projection.distanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
858
|
+
if (ambiguous) tieResolvedFlags[indexPosition] = 1;
|
|
859
|
+
}
|
|
860
|
+
|
|
861
|
+
return tieResolvedFlags;
|
|
862
|
+
}
|
|
863
|
+
|
|
864
|
+
function bestProjectionFromHintedSegmentNeighborhood(index, distanceAlong, radius, position, preferredDistance = null, target = null) {
|
|
865
|
+
const centerSegmentId = segmentIdAtDistance(index, distanceAlong);
|
|
866
|
+
if (!Number.isInteger(centerSegmentId)) return { projection: null, reason: 'no-candidates' };
|
|
867
|
+
return bestProjectionFromSegmentNeighborhood(
|
|
868
|
+
index.centerline,
|
|
869
|
+
centerSegmentId,
|
|
870
|
+
radius,
|
|
871
|
+
position,
|
|
872
|
+
preferredDistance,
|
|
873
|
+
target,
|
|
874
|
+
);
|
|
875
|
+
}
|
|
876
|
+
|
|
877
|
+
function segmentIdAtDistance(index, distanceAlong) {
|
|
878
|
+
const centerline = index?.centerline;
|
|
879
|
+
const segmentCount = centerline?.segmentCount ?? 0;
|
|
880
|
+
const totalLength = index?.arcBuckets?.totalLength;
|
|
881
|
+
if (segmentCount <= 0 || !Number.isFinite(distanceAlong) || !Number.isFinite(totalLength) || totalLength <= 0) return null;
|
|
882
|
+
const scratch = ensureQueryScratch(index);
|
|
883
|
+
if (scratch.hintDistanceCacheDistance === distanceAlong && Number.isInteger(scratch.hintDistanceCacheSegmentId) && scratch.hintDistanceCacheSegmentId >= 0) {
|
|
884
|
+
index.stats.hintDistanceCacheHits += 1;
|
|
885
|
+
return scratch.hintDistanceCacheSegmentId;
|
|
886
|
+
}
|
|
887
|
+
const wrapped = wrapDistance(distanceAlong, totalLength);
|
|
888
|
+
let low = 0;
|
|
889
|
+
let high = segmentCount - 1;
|
|
890
|
+
while (low < high) {
|
|
891
|
+
const mid = Math.floor((low + high) / 2);
|
|
892
|
+
if (centerline.endDistance[mid] < wrapped) low = mid + 1;
|
|
893
|
+
else high = mid;
|
|
894
|
+
}
|
|
895
|
+
scratch.hintDistanceCacheDistance = distanceAlong;
|
|
896
|
+
scratch.hintDistanceCacheWrappedDistance = wrapped;
|
|
897
|
+
scratch.hintDistanceCacheSegmentId = low;
|
|
898
|
+
scratch.hintDistanceCachePointX = Number.NaN;
|
|
899
|
+
scratch.hintDistanceCachePointY = Number.NaN;
|
|
900
|
+
return low;
|
|
901
|
+
}
|
|
902
|
+
|
|
903
|
+
function projectionTieScore(projection, preferredDistance) {
|
|
904
|
+
if (!Number.isFinite(preferredDistance)) return projection.segmentId;
|
|
905
|
+
return Math.abs(projection.distance - preferredDistance);
|
|
906
|
+
}
|
|
907
|
+
|
|
908
|
+
function projectionTieScoreForCandidate(distance, segmentId, preferredDistance) {
|
|
909
|
+
if (!Number.isFinite(preferredDistance)) return segmentId;
|
|
910
|
+
return Math.abs(distance - preferredDistance);
|
|
911
|
+
}
|
|
912
|
+
|
|
913
|
+
function projectionTieBeats(candidate, candidateTieScore, current, currentTieScore) {
|
|
914
|
+
if (candidateTieScore < currentTieScore - AMBIGUOUS_DISTANCE_EPSILON) return true;
|
|
915
|
+
if (Math.abs(candidateTieScore - currentTieScore) > AMBIGUOUS_DISTANCE_EPSILON) return false;
|
|
916
|
+
return candidate.segmentId < current.segmentId;
|
|
917
|
+
}
|
|
918
|
+
|
|
919
|
+
function projectionTieBeatsCandidate(candidateSegmentId, candidateTieScore, currentSegmentId, currentTieScore) {
|
|
920
|
+
if (candidateTieScore < currentTieScore - AMBIGUOUS_DISTANCE_EPSILON) return true;
|
|
921
|
+
if (Math.abs(candidateTieScore - currentTieScore) > AMBIGUOUS_DISTANCE_EPSILON) return false;
|
|
922
|
+
return candidateSegmentId < currentSegmentId;
|
|
923
|
+
}
|
|
924
|
+
|
|
925
|
+
function ensureFloat64ScratchArray(scratch, key, minimumLength) {
|
|
926
|
+
if (!(scratch[key] instanceof Float64Array) || scratch[key].length < minimumLength) {
|
|
927
|
+
scratch[key] = new Float64Array(Math.max(1, minimumLength));
|
|
928
|
+
}
|
|
929
|
+
return scratch[key];
|
|
930
|
+
}
|
|
931
|
+
|
|
932
|
+
function ensureUint8ScratchArray(scratch, key, minimumLength) {
|
|
933
|
+
if (!(scratch[key] instanceof Uint8Array) || scratch[key].length < minimumLength) {
|
|
934
|
+
scratch[key] = new Uint8Array(Math.max(1, minimumLength));
|
|
935
|
+
}
|
|
936
|
+
return scratch[key];
|
|
937
|
+
}
|
|
938
|
+
|
|
939
|
+
export function queryNearbyTrackProjections(track, position, { neighborLimit = GRID_NEIGHBOR_LIMIT } = {}) {
|
|
940
|
+
const index = track?.queryIndex;
|
|
941
|
+
if (!index?.centerline?.segmentCount || !finitePoint(position)) return null;
|
|
942
|
+
const ids = candidateIdsFromGrid(index, index.grid, position, neighborLimit);
|
|
943
|
+
if (!ids.length) return [];
|
|
944
|
+
return ids.map((segmentId) => projectIndexedSegment(index.centerline, segmentId, position));
|
|
945
|
+
}
|
|
946
|
+
|
|
947
|
+
export function queryTrackSegmentsInBounds(track, bounds) {
|
|
948
|
+
const index = track?.queryIndex;
|
|
949
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
950
|
+
if (!index?.centerline?.segmentCount || !grid || !finiteBounds(bounds)) return null;
|
|
951
|
+
const ids = candidateIdsFromGridBounds(index, grid, bounds);
|
|
952
|
+
if (!ids.length) return [];
|
|
953
|
+
return ids.map((segmentId) => segmentFromIndex(index.centerline, segmentId));
|
|
954
|
+
}
|
|
955
|
+
|
|
956
|
+
function isHintProjectionDistanceSafe(index, projection) {
|
|
957
|
+
const safeDistanceSquared = index.nearestNonLocalHintDistanceSquared?.[projection.segmentId] ?? 0;
|
|
958
|
+
if (!Number.isFinite(safeDistanceSquared)) return true;
|
|
959
|
+
return 4 * projection.crossTrackError * projection.crossTrackError < safeDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON;
|
|
960
|
+
}
|
|
961
|
+
|
|
962
|
+
function hintedQueryStaysInLocalCorridor(index, position, progressHint, maxOffset) {
|
|
963
|
+
const hintPoint = pointAtIndexedDistance(index, progressHint);
|
|
964
|
+
if (!hintPoint) return false;
|
|
965
|
+
const dx = position.x - hintPoint.x;
|
|
966
|
+
const dy = position.y - hintPoint.y;
|
|
967
|
+
return dx * dx + dy * dy <= maxOffset * maxOffset + AMBIGUOUS_DISTANCE_EPSILON;
|
|
968
|
+
}
|
|
969
|
+
|
|
970
|
+
function pointAtIndexedDistance(index, distanceAlong) {
|
|
971
|
+
const centerline = index?.centerline;
|
|
972
|
+
const scratch = ensureQueryScratch(index);
|
|
973
|
+
const segmentId = segmentIdAtDistance(index, distanceAlong);
|
|
974
|
+
if (!centerline || !Number.isInteger(segmentId)) return null;
|
|
975
|
+
if (
|
|
976
|
+
scratch.hintDistanceCacheDistance === distanceAlong &&
|
|
977
|
+
Number.isFinite(scratch.hintDistanceCachePointX) &&
|
|
978
|
+
Number.isFinite(scratch.hintDistanceCachePointY)
|
|
979
|
+
) {
|
|
980
|
+
return {
|
|
981
|
+
x: scratch.hintDistanceCachePointX,
|
|
982
|
+
y: scratch.hintDistanceCachePointY,
|
|
983
|
+
};
|
|
984
|
+
}
|
|
985
|
+
const startDistance = centerline.startDistance[segmentId];
|
|
986
|
+
const endDistance = centerline.endDistance[segmentId];
|
|
987
|
+
const span = Math.max(AMBIGUOUS_DISTANCE_EPSILON, endDistance - startDistance);
|
|
988
|
+
const wrappedDistance = scratch.hintDistanceCacheDistance === distanceAlong &&
|
|
989
|
+
Number.isFinite(scratch.hintDistanceCacheWrappedDistance)
|
|
990
|
+
? scratch.hintDistanceCacheWrappedDistance
|
|
991
|
+
: wrapDistance(distanceAlong, index.arcBuckets.totalLength);
|
|
992
|
+
const amount = clamp((wrappedDistance - startDistance) / span, 0, 1);
|
|
993
|
+
const point = {
|
|
994
|
+
x: centerline.startX[segmentId] + (centerline.endX[segmentId] - centerline.startX[segmentId]) * amount,
|
|
995
|
+
y: centerline.startY[segmentId] + (centerline.endY[segmentId] - centerline.startY[segmentId]) * amount,
|
|
996
|
+
};
|
|
997
|
+
scratch.hintDistanceCacheDistance = distanceAlong;
|
|
998
|
+
scratch.hintDistanceCacheWrappedDistance = wrappedDistance;
|
|
999
|
+
scratch.hintDistanceCacheSegmentId = segmentId;
|
|
1000
|
+
scratch.hintDistanceCachePointX = point.x;
|
|
1001
|
+
scratch.hintDistanceCachePointY = point.y;
|
|
1002
|
+
return point;
|
|
1003
|
+
}
|
|
1004
|
+
|
|
1005
|
+
function isHintProjectionLowDensitySafe(index, position) {
|
|
1006
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1007
|
+
const counts = index?.lowDensityHintRing1SegmentCounts;
|
|
1008
|
+
if (!grid || !counts?.length || !finitePoint(position)) return false;
|
|
1009
|
+
const cell = gridCellForPoint(grid, position, false);
|
|
1010
|
+
if (!cell) return false;
|
|
1011
|
+
const count = counts[cell.row * grid.columns + cell.column] ?? Infinity;
|
|
1012
|
+
return count <= LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD;
|
|
1013
|
+
}
|
|
1014
|
+
|
|
1015
|
+
function bestProjectionFromLowDensityCellNeighborhood(index, position, preferredDistance = null, target = null) {
|
|
1016
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1017
|
+
const counts = index?.lowDensityHintRing1SegmentCounts;
|
|
1018
|
+
const candidateIdsByCell = index?.lowDensityHintRing1CandidateIdsByCell;
|
|
1019
|
+
if (!grid || !counts?.length || !candidateIdsByCell?.length || !finitePoint(position)) {
|
|
1020
|
+
return { projection: null, reason: 'no-candidates' };
|
|
1021
|
+
}
|
|
1022
|
+
const center = gridCellForPoint(grid, position, false);
|
|
1023
|
+
if (!center) return { projection: null, reason: 'no-candidates' };
|
|
1024
|
+
const cellIndex = center.row * grid.columns + center.column;
|
|
1025
|
+
if ((counts[cellIndex] ?? Infinity) > LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD) {
|
|
1026
|
+
return { projection: null, reason: 'no-candidates' };
|
|
1027
|
+
}
|
|
1028
|
+
const candidateIds = candidateIdsByCell[cellIndex];
|
|
1029
|
+
if (!candidateIds?.length) return { projection: null, reason: 'no-candidates' };
|
|
1030
|
+
const local = bestProjectionFromCandidates(index.centerline, candidateIds, position, preferredDistance, target);
|
|
1031
|
+
if (!local.projection) return local;
|
|
1032
|
+
forEachRingCell(grid, center, 2, (_ringCellIndex, row, column) => {
|
|
1033
|
+
if (local.projection == null) return;
|
|
1034
|
+
if (
|
|
1035
|
+
row >= center.row - 1 &&
|
|
1036
|
+
row <= center.row + 1 &&
|
|
1037
|
+
column >= center.column - 1 &&
|
|
1038
|
+
column <= center.column + 1
|
|
1039
|
+
) return;
|
|
1040
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1041
|
+
if (!cell?.length) return;
|
|
1042
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
1043
|
+
if (lowerBound <= local.projection.distanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1044
|
+
local.projection = null;
|
|
1045
|
+
}
|
|
1046
|
+
});
|
|
1047
|
+
return local.projection ? local : { projection: null, reason: 'needs-wider-search' };
|
|
1048
|
+
}
|
|
1049
|
+
|
|
1050
|
+
function bestProjectionFromRing2Neighborhood(index, position, preferredDistance = null, target = null) {
|
|
1051
|
+
const grid = index?.segmentGrid ?? index?.grid;
|
|
1052
|
+
if (!grid || !finitePoint(position)) {
|
|
1053
|
+
return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1054
|
+
}
|
|
1055
|
+
const center = gridCellForPoint(grid, position, false);
|
|
1056
|
+
if (!center) return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1057
|
+
const candidateIds = candidateIdsFromCellNeighborhood(index, grid, center, 2);
|
|
1058
|
+
if (!candidateIds.length) return { projection: null, bestProjection: null, reason: 'no-candidates' };
|
|
1059
|
+
const local = bestProjectionFromCandidates(index.centerline, candidateIds, position, preferredDistance, target);
|
|
1060
|
+
if (!local.projection) return { projection: null, bestProjection: null, reason: local.reason ?? 'no-candidates' };
|
|
1061
|
+
const bestProjection = local.projection;
|
|
1062
|
+
forEachRingCell(grid, center, 3, (_cellIndex, row, column) => {
|
|
1063
|
+
if (local.projection == null) return;
|
|
1064
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1065
|
+
if (!cell?.length) return;
|
|
1066
|
+
const lowerBound = cellDistanceSquared(grid, row, column, position);
|
|
1067
|
+
if (lowerBound <= local.projection.distanceSquared + AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1068
|
+
local.projection = null;
|
|
1069
|
+
}
|
|
1070
|
+
});
|
|
1071
|
+
return local.projection
|
|
1072
|
+
? local
|
|
1073
|
+
: { projection: null, bestProjection, reason: 'needs-wider-search' };
|
|
1074
|
+
}
|
|
1075
|
+
|
|
1076
|
+
function candidateIdsFromCellNeighborhood(index, grid, center, radius) {
|
|
1077
|
+
const scratch = ensureQueryScratch(index);
|
|
1078
|
+
const segmentMarks = ensureScratchArray(scratch, 'cellNeighborhoodSegmentMarks', index.centerline.segmentCount);
|
|
1079
|
+
const segmentEpoch = nextScratchEpoch(scratch, 'cellNeighborhoodSegmentEpoch', segmentMarks);
|
|
1080
|
+
const ids = scratch.cellNeighborhoodSegmentIds ?? [];
|
|
1081
|
+
ids.length = 0;
|
|
1082
|
+
for (let row = Math.max(0, center.row - radius); row <= Math.min(grid.rows - 1, center.row + radius); row += 1) {
|
|
1083
|
+
for (let column = Math.max(0, center.column - radius); column <= Math.min(grid.columns - 1, center.column + radius); column += 1) {
|
|
1084
|
+
const cell = grid.cells[row * grid.columns + column];
|
|
1085
|
+
if (!cell) continue;
|
|
1086
|
+
for (const segmentId of cell) {
|
|
1087
|
+
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
1088
|
+
segmentMarks[segmentId] = segmentEpoch;
|
|
1089
|
+
ids.push(segmentId);
|
|
1090
|
+
}
|
|
1091
|
+
}
|
|
1092
|
+
}
|
|
1093
|
+
scratch.cellNeighborhoodSegmentIds = ids;
|
|
1094
|
+
return ids;
|
|
1095
|
+
}
|
|
1096
|
+
|
|
1097
|
+
function bestProjectionFromArcBuckets(index, distanceAlong, radius, position, preferredDistance = null, target = null) {
|
|
1098
|
+
const arcBuckets = index?.arcBuckets;
|
|
1099
|
+
if (!arcBuckets?.count || !Number.isFinite(distanceAlong)) return { projection: null, reason: 'no-candidates' };
|
|
1100
|
+
const wrapped = wrapDistance(distanceAlong, arcBuckets.totalLength);
|
|
1101
|
+
const center = Math.floor(wrapped / arcBuckets.bucketLength);
|
|
1102
|
+
const precomputedRadius2Candidates = radius === 2
|
|
1103
|
+
? arcBuckets.radius2CandidateIdsByBucket?.[center] ?? null
|
|
1104
|
+
: null;
|
|
1105
|
+
const scratch = precomputedRadius2Candidates ? null : ensureQueryScratch(index);
|
|
1106
|
+
const segmentMarks = precomputedRadius2Candidates
|
|
1107
|
+
? null
|
|
1108
|
+
: ensureScratchArray(scratch, 'candidateMarks', index.centerline.segmentCount);
|
|
1109
|
+
const candidateEpoch = precomputedRadius2Candidates
|
|
1110
|
+
? 0
|
|
1111
|
+
: nextScratchEpoch(scratch, 'candidateEpoch', segmentMarks);
|
|
1112
|
+
let bestSegmentId = null;
|
|
1113
|
+
let bestX = 0;
|
|
1114
|
+
let bestY = 0;
|
|
1115
|
+
let bestDistance = 0;
|
|
1116
|
+
let bestHeading = 0;
|
|
1117
|
+
let bestNormalX = 0;
|
|
1118
|
+
let bestNormalY = 0;
|
|
1119
|
+
let bestCurvature = 0;
|
|
1120
|
+
let bestSignedOffset = 0;
|
|
1121
|
+
let bestDistanceSquared = Infinity;
|
|
1122
|
+
let bestTieScore = Infinity;
|
|
1123
|
+
let secondBestDistance = Infinity;
|
|
1124
|
+
let tieResolved = false;
|
|
1125
|
+
|
|
1126
|
+
const visitCandidate = (segmentId) => {
|
|
1127
|
+
if (segmentMarks) {
|
|
1128
|
+
if (segmentMarks[segmentId] === candidateEpoch) return;
|
|
1129
|
+
segmentMarks[segmentId] = candidateEpoch;
|
|
1130
|
+
}
|
|
1131
|
+
const ax = index.centerline.startX[segmentId];
|
|
1132
|
+
const ay = index.centerline.startY[segmentId];
|
|
1133
|
+
const dx = index.centerline.deltaX[segmentId];
|
|
1134
|
+
const dy = index.centerline.deltaY[segmentId];
|
|
1135
|
+
const amount = index.centerline.lengthSquared[segmentId] > 0
|
|
1136
|
+
? clamp(((position.x - ax) * dx + (position.y - ay) * dy) / index.centerline.lengthSquared[segmentId], 0, 1)
|
|
1137
|
+
: 0;
|
|
1138
|
+
const x = ax + dx * amount;
|
|
1139
|
+
const y = ay + dy * amount;
|
|
1140
|
+
const distance = index.centerline.startDistance[segmentId] +
|
|
1141
|
+
index.centerline.distanceSpan[segmentId] * amount;
|
|
1142
|
+
const normalX = index.centerline.normalX[segmentId];
|
|
1143
|
+
const normalY = index.centerline.normalY[segmentId];
|
|
1144
|
+
const px = position.x - x;
|
|
1145
|
+
const py = position.y - y;
|
|
1146
|
+
const signedOffset = px * normalX + py * normalY;
|
|
1147
|
+
const distanceSquared = px * px + py * py;
|
|
1148
|
+
const tieScore = projectionTieScoreForCandidate(distance, segmentId, preferredDistance);
|
|
1149
|
+
if (bestSegmentId == null || distanceSquared < bestDistanceSquared - AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1150
|
+
secondBestDistance = bestSegmentId == null ? Infinity : bestDistanceSquared;
|
|
1151
|
+
bestSegmentId = segmentId;
|
|
1152
|
+
bestX = x;
|
|
1153
|
+
bestY = y;
|
|
1154
|
+
bestDistance = distance;
|
|
1155
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
1156
|
+
bestNormalX = normalX;
|
|
1157
|
+
bestNormalY = normalY;
|
|
1158
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
1159
|
+
bestSignedOffset = signedOffset;
|
|
1160
|
+
bestDistanceSquared = distanceSquared;
|
|
1161
|
+
bestTieScore = tieScore;
|
|
1162
|
+
} else if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
1163
|
+
tieResolved = true;
|
|
1164
|
+
secondBestDistance = Math.min(secondBestDistance, distanceSquared);
|
|
1165
|
+
if (projectionTieBeatsCandidate(segmentId, tieScore, bestSegmentId, bestTieScore)) {
|
|
1166
|
+
bestSegmentId = segmentId;
|
|
1167
|
+
bestX = x;
|
|
1168
|
+
bestY = y;
|
|
1169
|
+
bestDistance = distance;
|
|
1170
|
+
bestHeading = index.centerline.heading[segmentId];
|
|
1171
|
+
bestNormalX = normalX;
|
|
1172
|
+
bestNormalY = normalY;
|
|
1173
|
+
bestCurvature = index.centerline.curvature[segmentId];
|
|
1174
|
+
bestSignedOffset = signedOffset;
|
|
1175
|
+
bestDistanceSquared = distanceSquared;
|
|
1176
|
+
bestTieScore = tieScore;
|
|
1177
|
+
}
|
|
1178
|
+
} else if (distanceSquared < secondBestDistance) {
|
|
1179
|
+
secondBestDistance = distanceSquared;
|
|
1180
|
+
}
|
|
1181
|
+
};
|
|
1182
|
+
|
|
1183
|
+
if (precomputedRadius2Candidates) {
|
|
1184
|
+
index.stats.arcBucketRadius2PrecomputedQueries += 1;
|
|
1185
|
+
for (const segmentId of precomputedRadius2Candidates) visitCandidate(segmentId);
|
|
1186
|
+
} else {
|
|
1187
|
+
for (let offset = -radius; offset <= radius; offset += 1) {
|
|
1188
|
+
const bucket = ((center + offset) % arcBuckets.count + arcBuckets.count) % arcBuckets.count;
|
|
1189
|
+
for (const segmentId of arcBuckets.buckets[bucket]) visitCandidate(segmentId);
|
|
1190
|
+
}
|
|
1191
|
+
}
|
|
1192
|
+
|
|
1193
|
+
if (bestSegmentId == null) return { projection: null, reason: 'no-best' };
|
|
1194
|
+
const projection = writeProjectionCandidate(target ?? {}, {
|
|
1195
|
+
segmentId: bestSegmentId,
|
|
1196
|
+
x: bestX,
|
|
1197
|
+
y: bestY,
|
|
1198
|
+
distance: bestDistance,
|
|
1199
|
+
heading: bestHeading,
|
|
1200
|
+
normalX: bestNormalX,
|
|
1201
|
+
normalY: bestNormalY,
|
|
1202
|
+
curvature: bestCurvature,
|
|
1203
|
+
signedOffset: bestSignedOffset,
|
|
1204
|
+
distanceSquared: bestDistanceSquared,
|
|
1205
|
+
});
|
|
1206
|
+
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
1207
|
+
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
1208
|
+
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
1209
|
+
}
|
|
1210
|
+
|
|
1211
|
+
function finiteBounds(bounds) {
|
|
1212
|
+
return Number.isFinite(bounds?.minX) &&
|
|
1213
|
+
Number.isFinite(bounds?.maxX) &&
|
|
1214
|
+
Number.isFinite(bounds?.minY) &&
|
|
1215
|
+
Number.isFinite(bounds?.maxY) &&
|
|
1216
|
+
bounds.minX <= bounds.maxX &&
|
|
1217
|
+
bounds.minY <= bounds.maxY;
|
|
1218
|
+
}
|