@inventure71/paddockjs 4.2.0 → 4.3.1
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 +40 -0
- package/INSTALL_AND_UPDATE.md +24 -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 +84 -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 +38 -11
- package/docs/testing-reliability-guide.md +5 -0
- package/docs/v11_improvements.md +108 -0
- package/package.json +24 -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 +106 -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
|
@@ -1,1938 +1,17 @@
|
|
|
1
|
-
|
|
2
|
-
|
|
3
|
-
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
|
|
7
|
-
|
|
8
|
-
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
1
|
+
export { createTrackQueryIndex, attachTrackQueryIndex, forkTrackQueryIndex } from './trackQueryConstruction.js';
|
|
2
|
+
export {
|
|
3
|
+
queryNearestTrackProjection,
|
|
4
|
+
queryNearestTrackProjectionInto,
|
|
5
|
+
queryHintedTrackProjection,
|
|
6
|
+
querySegmentNeighborhoodProjection,
|
|
7
|
+
querySegmentNeighborhoodProjections,
|
|
8
|
+
queryNearbyTrackProjections,
|
|
9
|
+
queryTrackSegmentsInBounds,
|
|
10
|
+
} from './trackNearestQueries.js';
|
|
11
|
+
export { queryTrackSegmentIdsAlongRay, queryTrackSegmentsAlongRay } from './trackRayQueries.js';
|
|
12
|
+
export { resetTrackQueryStats, snapshotTrackQueryStats } from './trackQueryStats.js';
|
|
14
13
|
export {
|
|
15
14
|
queryPitBoxCandidates,
|
|
16
15
|
queryPitRoadSegmentCandidates,
|
|
17
16
|
queryPitRoadSegmentCandidatesByRoute,
|
|
18
17
|
} from './trackQueryPitIndex.js';
|
|
19
|
-
|
|
20
|
-
const DEFAULT_GRID_CELL_SIZE = metersToSimUnits(32);
|
|
21
|
-
const GRID_NEIGHBOR_LIMIT = 2;
|
|
22
|
-
const ARC_BUCKET_COUNT = 512;
|
|
23
|
-
const AMBIGUOUS_DISTANCE_EPSILON = 1e-6;
|
|
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;
|
|
28
|
-
|
|
29
|
-
export function createTrackQueryIndex(track) {
|
|
30
|
-
const samples = Array.isArray(track.samples) ? track.samples : [];
|
|
31
|
-
const segmentCount = Math.max(0, samples.length - 1);
|
|
32
|
-
const bands = createTrackBands(track);
|
|
33
|
-
const expansion = bands.runoffEdge + (track.barrierWidth ?? 0) + metersToSimUnits(64);
|
|
34
|
-
const bounds = expandedSampleBounds(samples, expansion);
|
|
35
|
-
const centerline = createCenterlineSegments(samples, segmentCount);
|
|
36
|
-
centerline.precomputedNeighborhoodsByRadius = createPrecomputedSegmentNeighborhoods(segmentCount, PRECOMPUTED_SEGMENT_NEIGHBORHOOD_RADII);
|
|
37
|
-
const grid = createSpatialGrid(bounds, DEFAULT_GRID_CELL_SIZE);
|
|
38
|
-
const segmentGrid = createSpatialGrid(bounds, DEFAULT_GRID_CELL_SIZE);
|
|
39
|
-
const arcBuckets = createArcBuckets(track.length, ARC_BUCKET_COUNT);
|
|
40
|
-
|
|
41
|
-
for (let segmentId = 0; segmentId < segmentCount; segmentId += 1) {
|
|
42
|
-
insertSegmentIntoGrid(grid, centerline, segmentId, expansion);
|
|
43
|
-
insertSegmentIntoGrid(segmentGrid, centerline, segmentId, 0);
|
|
44
|
-
insertSegmentIntoArcBuckets(arcBuckets, centerline, segmentId);
|
|
45
|
-
}
|
|
46
|
-
finalizeArcBuckets(arcBuckets, segmentCount);
|
|
47
|
-
|
|
48
|
-
const lowDensityHintRing1Neighborhood = createLowDensityHintRing1Neighborhood(centerline, segmentGrid, LOW_DENSITY_HINT_RING1_SEGMENT_THRESHOLD);
|
|
49
|
-
const index = {
|
|
50
|
-
version: 1,
|
|
51
|
-
bands,
|
|
52
|
-
nearestGridExpansion: expansion,
|
|
53
|
-
centerline,
|
|
54
|
-
grid,
|
|
55
|
-
segmentGrid,
|
|
56
|
-
arcBuckets,
|
|
57
|
-
nearestOccupiedCellRadiusByCell: createNearestOccupiedCellRadiusByCell(segmentGrid),
|
|
58
|
-
nearestNonLocalHintDistanceSquared: createNearestNonLocalHintDistanceSquared(centerline, segmentGrid, bands.kerbEdge * 2),
|
|
59
|
-
lowDensityHintRing1SegmentCounts: lowDensityHintRing1Neighborhood.counts,
|
|
60
|
-
lowDensityHintRing1CandidateIdsByCell: lowDensityHintRing1Neighborhood.candidateIdsByCell,
|
|
61
|
-
pit: createPitQueryIndex(track, grid.cellSize),
|
|
62
|
-
stats: createQueryStats(),
|
|
63
|
-
};
|
|
64
|
-
attachQueryStatsToCenterline(centerline, index.stats);
|
|
65
|
-
index.queryScratch = createQueryScratch(index);
|
|
66
|
-
return index;
|
|
67
|
-
}
|
|
68
|
-
|
|
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) {
|
|
74
|
-
const index = track?.queryIndex;
|
|
75
|
-
if (!index?.centerline?.segmentCount) {
|
|
76
|
-
recordFallback(index, 'nearestFallbackReasons', 'missing-index');
|
|
77
|
-
return null;
|
|
78
|
-
}
|
|
79
|
-
if (!finitePoint(position)) {
|
|
80
|
-
recordFallback(index, 'nearestFallbackReasons', 'invalid-position');
|
|
81
|
-
return null;
|
|
82
|
-
}
|
|
83
|
-
index.stats.nearestQueries += 1;
|
|
84
|
-
|
|
85
|
-
const { projection, path, reason } = bestProjectionFromIndex(index, position, progressHint, target);
|
|
86
|
-
if (!projection) {
|
|
87
|
-
recordFallback(index, 'nearestFallbackReasons', reason ?? 'unknown');
|
|
88
|
-
return null;
|
|
89
|
-
}
|
|
90
|
-
|
|
91
|
-
recordStat(index, 'nearestPaths', path);
|
|
92
|
-
return projection;
|
|
93
|
-
}
|
|
94
|
-
|
|
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;
|
|
199
|
-
}
|
|
200
|
-
|
|
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;
|
|
235
|
-
return {
|
|
236
|
-
projection: lowDensityExact.projection,
|
|
237
|
-
path: lowDensityExact.reason === 'tie-resolved'
|
|
238
|
-
? 'low-density-cell-exact-tie-resolved'
|
|
239
|
-
: 'low-density-cell-exact',
|
|
240
|
-
};
|
|
241
|
-
}
|
|
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
|
-
}
|
|
289
|
-
}
|
|
290
|
-
|
|
291
|
-
let local = { projection: null, path: null };
|
|
292
|
-
const gridCandidateIds = candidateIdsFromGrid(index, index.grid, position, GRID_NEIGHBOR_LIMIT);
|
|
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`;
|
|
307
|
-
return {
|
|
308
|
-
projection: exact.projection,
|
|
309
|
-
path: exact.reason === 'tie-resolved' && !path.endsWith('tie-resolved') ? `${path}-tie-resolved` : path,
|
|
310
|
-
};
|
|
311
|
-
}
|
|
312
|
-
|
|
313
|
-
return {
|
|
314
|
-
projection: local.projection,
|
|
315
|
-
path: local.path,
|
|
316
|
-
};
|
|
317
|
-
}
|
|
318
|
-
|
|
319
|
-
const exact = bestProjectionFromCellSearch(index, position, progressHint);
|
|
320
|
-
const global = exact.projection ? exact : bestProjectionFromAllSegments(index.centerline, position, progressHint, target);
|
|
321
|
-
return global.projection
|
|
322
|
-
? {
|
|
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'),
|
|
327
|
-
}
|
|
328
|
-
: { projection: null, reason: `global-index-${global.reason}` };
|
|
329
|
-
}
|
|
330
|
-
|
|
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;
|
|
346
|
-
}
|
|
347
|
-
|
|
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
|
-
|
|
357
|
-
const scratch = ensureQueryScratch(index);
|
|
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
|
-
}
|
|
502
|
-
|
|
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;
|
|
514
|
-
let bestDistanceSquared = Infinity;
|
|
515
|
-
let bestTieScore = Infinity;
|
|
516
|
-
let secondBestDistance = Infinity;
|
|
517
|
-
let tieResolved = false;
|
|
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
|
-
});
|
|
584
|
-
|
|
585
|
-
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
586
|
-
Math.abs(secondBestDistance - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON;
|
|
587
|
-
return { projection, reason: ambiguous || tieResolved ? 'tie-resolved' : 'ok' };
|
|
588
|
-
}
|
|
589
|
-
|
|
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;
|
|
637
|
-
bestDistanceSquared = distanceSquared;
|
|
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;
|
|
657
|
-
}
|
|
658
|
-
}
|
|
659
|
-
|
|
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' };
|
|
676
|
-
}
|
|
677
|
-
|
|
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;
|
|
698
|
-
let bestTieScore = Infinity;
|
|
699
|
-
let secondBestDistance = Infinity;
|
|
700
|
-
let tieResolved = false;
|
|
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;
|
|
741
|
-
bestTieScore = tieScore;
|
|
742
|
-
continue;
|
|
743
|
-
}
|
|
744
|
-
|
|
745
|
-
if (Math.abs(distanceSquared - bestDistanceSquared) <= AMBIGUOUS_DISTANCE_EPSILON) {
|
|
746
|
-
tieResolved = true;
|
|
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;
|
|
759
|
-
bestTieScore = tieScore;
|
|
760
|
-
}
|
|
761
|
-
continue;
|
|
762
|
-
}
|
|
763
|
-
|
|
764
|
-
if (distanceSquared < secondBestDistance) {
|
|
765
|
-
secondBestDistance = distanceSquared;
|
|
766
|
-
}
|
|
767
|
-
}
|
|
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;
|
|
782
|
-
const ambiguous = Number.isFinite(secondBestDistance) &&
|
|
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;
|
|
985
|
-
}
|
|
986
|
-
|
|
987
|
-
function projectionTieScore(projection, preferredDistance) {
|
|
988
|
-
if (!Number.isFinite(preferredDistance)) return projection.segmentId;
|
|
989
|
-
return Math.abs(projection.distance - preferredDistance);
|
|
990
|
-
}
|
|
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
|
-
|
|
1023
|
-
export function queryNearbyTrackProjections(track, position, { neighborLimit = GRID_NEIGHBOR_LIMIT } = {}) {
|
|
1024
|
-
const index = track?.queryIndex;
|
|
1025
|
-
if (!index?.centerline?.segmentCount || !finitePoint(position)) return null;
|
|
1026
|
-
const ids = candidateIdsFromGrid(index, index.grid, position, neighborLimit);
|
|
1027
|
-
if (!ids.length) return [];
|
|
1028
|
-
return ids.map((segmentId) => projectIndexedSegment(index.centerline, segmentId, position));
|
|
1029
|
-
}
|
|
1030
|
-
|
|
1031
|
-
export function queryTrackSegmentsInBounds(track, bounds) {
|
|
1032
|
-
const index = track?.queryIndex;
|
|
1033
|
-
const grid = index?.segmentGrid ?? index?.grid;
|
|
1034
|
-
if (!index?.centerline?.segmentCount || !grid || !finiteBounds(bounds)) return null;
|
|
1035
|
-
const ids = candidateIdsFromGridBounds(index, grid, bounds);
|
|
1036
|
-
if (!ids.length) return [];
|
|
1037
|
-
return ids.map((segmentId) => segmentFromIndex(index.centerline, segmentId));
|
|
1038
|
-
}
|
|
1039
|
-
|
|
1040
|
-
export function queryTrackSegmentIdsAlongRay(track, origin, vector, maxDistance, margin = 0) {
|
|
1041
|
-
const index = track?.queryIndex;
|
|
1042
|
-
if (
|
|
1043
|
-
!index?.centerline?.segmentCount ||
|
|
1044
|
-
!finitePoint(origin) ||
|
|
1045
|
-
!Number.isFinite(vector?.x) ||
|
|
1046
|
-
!Number.isFinite(vector?.y) ||
|
|
1047
|
-
!Number.isFinite(maxDistance) ||
|
|
1048
|
-
maxDistance < 0
|
|
1049
|
-
) return null;
|
|
1050
|
-
|
|
1051
|
-
const grid = index.segmentGrid ?? index.grid;
|
|
1052
|
-
const scratch = ensureQueryScratch(index);
|
|
1053
|
-
const segmentMarks = ensureScratchArray(scratch, 'raySegmentMarks', index.centerline.segmentCount);
|
|
1054
|
-
const cellMarks = ensureScratchArray(scratch, 'rayCellMarks', grid.columns * grid.rows);
|
|
1055
|
-
const segmentEpoch = nextScratchEpoch(scratch, 'raySegmentEpoch', segmentMarks);
|
|
1056
|
-
const cellEpoch = nextScratchEpoch(scratch, 'rayCellEpoch', cellMarks);
|
|
1057
|
-
const ids = scratch.raySegmentIds;
|
|
1058
|
-
ids.length = 0;
|
|
1059
|
-
const radius = Math.max(0, Math.ceil(Math.max(0, margin) / grid.cellSize));
|
|
1060
|
-
const step = Math.max(grid.cellSize * 0.5, 1);
|
|
1061
|
-
const sampleCount = Math.max(1, Math.ceil(maxDistance / step));
|
|
1062
|
-
const visitedCells = scratch.rayVisitedCells;
|
|
1063
|
-
visitedCells.length = 0;
|
|
1064
|
-
const point = scratch.rayTracePoint ?? { x: 0, y: 0 };
|
|
1065
|
-
scratch.rayTracePoint = point;
|
|
1066
|
-
|
|
1067
|
-
const seed = seedRayTraceFromNearbyCache(index, grid, origin, vector, radius, maxDistance, segmentMarks, segmentEpoch, cellMarks, cellEpoch, ids);
|
|
1068
|
-
|
|
1069
|
-
for (let sample = 0; sample <= sampleCount; sample += 1) {
|
|
1070
|
-
const distance = Math.min(maxDistance, sample * step);
|
|
1071
|
-
point.x = origin.x + vector.x * distance;
|
|
1072
|
-
point.y = origin.y + vector.y * distance;
|
|
1073
|
-
const center = gridCellForPoint(grid, point, false);
|
|
1074
|
-
if (!center) continue;
|
|
1075
|
-
for (let row = center.row - radius; row <= center.row + radius; row += 1) {
|
|
1076
|
-
for (let column = center.column - radius; column <= center.column + radius; column += 1) {
|
|
1077
|
-
if (column < 0 || row < 0 || column >= grid.columns || row >= grid.rows) continue;
|
|
1078
|
-
const cellIndex = row * grid.columns + column;
|
|
1079
|
-
if (cellMarks[cellIndex] === cellEpoch) continue;
|
|
1080
|
-
cellMarks[cellIndex] = cellEpoch;
|
|
1081
|
-
visitedCells.push(cellIndex);
|
|
1082
|
-
const cell = grid.cells[cellIndex];
|
|
1083
|
-
if (!cell) continue;
|
|
1084
|
-
for (const segmentId of cell) {
|
|
1085
|
-
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
1086
|
-
segmentMarks[segmentId] = segmentEpoch;
|
|
1087
|
-
ids.push(segmentId);
|
|
1088
|
-
}
|
|
1089
|
-
}
|
|
1090
|
-
}
|
|
1091
|
-
}
|
|
1092
|
-
|
|
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;
|
|
1101
|
-
if (!ids.length) return [];
|
|
1102
|
-
return ids.map((segmentId) => segmentFromIndex(index.centerline, segmentId));
|
|
1103
|
-
}
|
|
1104
|
-
|
|
1105
|
-
export function attachTrackQueryIndex(track, queryIndex) {
|
|
1106
|
-
Object.defineProperty(track, 'queryIndex', {
|
|
1107
|
-
configurable: true,
|
|
1108
|
-
enumerable: false,
|
|
1109
|
-
value: queryIndex,
|
|
1110
|
-
writable: false,
|
|
1111
|
-
});
|
|
1112
|
-
return track;
|
|
1113
|
-
}
|
|
1114
|
-
|
|
1115
|
-
export function forkTrackQueryIndex(sourceIndex, runtimeTrack = null) {
|
|
1116
|
-
if (!sourceIndex || typeof sourceIndex !== 'object') return null;
|
|
1117
|
-
const index = {
|
|
1118
|
-
...sourceIndex,
|
|
1119
|
-
pit: runtimeTrack ? createPitQueryIndex(runtimeTrack, sourceIndex.grid?.cellSize ?? DEFAULT_GRID_CELL_SIZE) : sourceIndex.pit,
|
|
1120
|
-
stats: createQueryStats(),
|
|
1121
|
-
};
|
|
1122
|
-
attachQueryStatsToCenterline(index.centerline, index.stats);
|
|
1123
|
-
index.queryScratch = createQueryScratch(index);
|
|
1124
|
-
return index;
|
|
1125
|
-
}
|
|
1126
|
-
|
|
1127
|
-
export function resetTrackQueryStats(track) {
|
|
1128
|
-
const stats = track?.queryIndex?.stats;
|
|
1129
|
-
if (!stats) return null;
|
|
1130
|
-
Object.assign(stats, createQueryStats());
|
|
1131
|
-
return stats;
|
|
1132
|
-
}
|
|
1133
|
-
|
|
1134
|
-
export function snapshotTrackQueryStats(track) {
|
|
1135
|
-
const stats = track?.queryIndex?.stats;
|
|
1136
|
-
return stats ? JSON.parse(JSON.stringify(stats)) : null;
|
|
1137
|
-
}
|
|
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
|
-
|
|
1153
|
-
function createTrackBands(track) {
|
|
1154
|
-
const trackEdge = track.width / 2;
|
|
1155
|
-
const kerbEdge = trackEdge + (track.kerbWidth ?? 0);
|
|
1156
|
-
const gravelEdge = kerbEdge + (track.gravelWidth ?? 0);
|
|
1157
|
-
const runoffEdge = gravelEdge + (track.runoffWidth ?? 0);
|
|
1158
|
-
const barrierInnerFace = runoffEdge - (track.barrierWidth ?? 0) / 2;
|
|
1159
|
-
return {
|
|
1160
|
-
trackEdge,
|
|
1161
|
-
kerbEdge,
|
|
1162
|
-
gravelEdge,
|
|
1163
|
-
runoffEdge,
|
|
1164
|
-
barrierInnerFace,
|
|
1165
|
-
};
|
|
1166
|
-
}
|
|
1167
|
-
|
|
1168
|
-
function createCenterlineSegments(samples, segmentCount) {
|
|
1169
|
-
const startX = new Float64Array(segmentCount);
|
|
1170
|
-
const startY = new Float64Array(segmentCount);
|
|
1171
|
-
const endX = new Float64Array(segmentCount);
|
|
1172
|
-
const endY = new Float64Array(segmentCount);
|
|
1173
|
-
const deltaX = new Float64Array(segmentCount);
|
|
1174
|
-
const deltaY = new Float64Array(segmentCount);
|
|
1175
|
-
const lengthSquared = new Float64Array(segmentCount);
|
|
1176
|
-
const startDistance = new Float64Array(segmentCount);
|
|
1177
|
-
const endDistance = new Float64Array(segmentCount);
|
|
1178
|
-
const distanceSpan = new Float64Array(segmentCount);
|
|
1179
|
-
const heading = new Float64Array(segmentCount);
|
|
1180
|
-
const normalX = new Float64Array(segmentCount);
|
|
1181
|
-
const normalY = new Float64Array(segmentCount);
|
|
1182
|
-
const endNormalX = new Float64Array(segmentCount);
|
|
1183
|
-
const endNormalY = new Float64Array(segmentCount);
|
|
1184
|
-
const curvature = new Float64Array(segmentCount);
|
|
1185
|
-
const minX = new Float64Array(segmentCount);
|
|
1186
|
-
const maxX = new Float64Array(segmentCount);
|
|
1187
|
-
const minY = new Float64Array(segmentCount);
|
|
1188
|
-
const maxY = new Float64Array(segmentCount);
|
|
1189
|
-
|
|
1190
|
-
for (let id = 0; id < segmentCount; id += 1) {
|
|
1191
|
-
const start = samples[id];
|
|
1192
|
-
const end = samples[id + 1];
|
|
1193
|
-
startX[id] = start.x;
|
|
1194
|
-
startY[id] = start.y;
|
|
1195
|
-
endX[id] = end.x;
|
|
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];
|
|
1200
|
-
startDistance[id] = start.distance;
|
|
1201
|
-
endDistance[id] = end.distance;
|
|
1202
|
-
distanceSpan[id] = end.distance - start.distance;
|
|
1203
|
-
heading[id] = start.heading;
|
|
1204
|
-
normalX[id] = start.normalX;
|
|
1205
|
-
normalY[id] = start.normalY;
|
|
1206
|
-
endNormalX[id] = end.normalX;
|
|
1207
|
-
endNormalY[id] = end.normalY;
|
|
1208
|
-
curvature[id] = start.curvature ?? 0;
|
|
1209
|
-
minX[id] = Math.min(start.x, end.x);
|
|
1210
|
-
maxX[id] = Math.max(start.x, end.x);
|
|
1211
|
-
minY[id] = Math.min(start.y, end.y);
|
|
1212
|
-
maxY[id] = Math.max(start.y, end.y);
|
|
1213
|
-
}
|
|
1214
|
-
|
|
1215
|
-
return {
|
|
1216
|
-
segmentCount,
|
|
1217
|
-
startX,
|
|
1218
|
-
startY,
|
|
1219
|
-
endX,
|
|
1220
|
-
endY,
|
|
1221
|
-
deltaX,
|
|
1222
|
-
deltaY,
|
|
1223
|
-
lengthSquared,
|
|
1224
|
-
startDistance,
|
|
1225
|
-
endDistance,
|
|
1226
|
-
distanceSpan,
|
|
1227
|
-
heading,
|
|
1228
|
-
normalX,
|
|
1229
|
-
normalY,
|
|
1230
|
-
endNormalX,
|
|
1231
|
-
endNormalY,
|
|
1232
|
-
curvature,
|
|
1233
|
-
minX,
|
|
1234
|
-
maxX,
|
|
1235
|
-
minY,
|
|
1236
|
-
maxY,
|
|
1237
|
-
};
|
|
1238
|
-
}
|
|
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
|
-
|
|
1263
|
-
function createArcBuckets(totalLength, count) {
|
|
1264
|
-
return {
|
|
1265
|
-
count,
|
|
1266
|
-
totalLength,
|
|
1267
|
-
bucketLength: totalLength / count,
|
|
1268
|
-
buckets: Array.from({ length: count }, () => []),
|
|
1269
|
-
radius2CandidateIdsByBucket: Array.from({ length: count }, () => []),
|
|
1270
|
-
};
|
|
1271
|
-
}
|
|
1272
|
-
|
|
1273
|
-
function insertSegmentIntoArcBuckets(arcBuckets, centerline, segmentId) {
|
|
1274
|
-
if (!Number.isFinite(arcBuckets.bucketLength) || arcBuckets.bucketLength <= 0) return;
|
|
1275
|
-
const start = Math.floor(centerline.startDistance[segmentId] / arcBuckets.bucketLength);
|
|
1276
|
-
const end = Math.floor(centerline.endDistance[segmentId] / arcBuckets.bucketLength);
|
|
1277
|
-
for (let bucket = start; bucket <= end; bucket += 1) {
|
|
1278
|
-
arcBuckets.buckets[((bucket % arcBuckets.count) + arcBuckets.count) % arcBuckets.count].push(segmentId);
|
|
1279
|
-
}
|
|
1280
|
-
}
|
|
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
|
-
|
|
1305
|
-
function expandedSampleBounds(samples, expansion) {
|
|
1306
|
-
const bounds = samples.reduce((current, point) => ({
|
|
1307
|
-
minX: Math.min(current.minX, point.x),
|
|
1308
|
-
maxX: Math.max(current.maxX, point.x),
|
|
1309
|
-
minY: Math.min(current.minY, point.y),
|
|
1310
|
-
maxY: Math.max(current.maxY, point.y),
|
|
1311
|
-
}), { minX: Infinity, maxX: -Infinity, minY: Infinity, maxY: -Infinity });
|
|
1312
|
-
return {
|
|
1313
|
-
minX: bounds.minX - expansion,
|
|
1314
|
-
maxX: bounds.maxX + expansion,
|
|
1315
|
-
minY: bounds.minY - expansion,
|
|
1316
|
-
maxY: bounds.maxY + expansion,
|
|
1317
|
-
};
|
|
1318
|
-
}
|
|
1319
|
-
|
|
1320
|
-
function insertSegmentIntoGrid(grid, centerline, segmentId, expansion) {
|
|
1321
|
-
insertIdIntoGridBounds(grid, segmentId, {
|
|
1322
|
-
minX: centerline.minX[segmentId] - expansion,
|
|
1323
|
-
maxX: centerline.maxX[segmentId] + expansion,
|
|
1324
|
-
minY: centerline.minY[segmentId] - expansion,
|
|
1325
|
-
maxY: centerline.maxY[segmentId] + expansion,
|
|
1326
|
-
});
|
|
1327
|
-
}
|
|
1328
|
-
|
|
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) {
|
|
1613
|
-
const scratch = ensureQueryScratch(index);
|
|
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' };
|
|
1670
|
-
const wrapped = wrapDistance(distanceAlong, arcBuckets.totalLength);
|
|
1671
|
-
const center = Math.floor(wrapped / arcBuckets.bucketLength);
|
|
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;
|
|
1699
|
-
segmentMarks[segmentId] = candidateEpoch;
|
|
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);
|
|
1760
|
-
}
|
|
1761
|
-
}
|
|
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' };
|
|
1779
|
-
}
|
|
1780
|
-
|
|
1781
|
-
function projectIndexedSegment(centerline, segmentId, position) {
|
|
1782
|
-
if (centerline?.stats) centerline.stats.candidateProjectionObjectAllocations += 1;
|
|
1783
|
-
const ax = centerline.startX[segmentId];
|
|
1784
|
-
const ay = centerline.startY[segmentId];
|
|
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)
|
|
1789
|
-
: 0;
|
|
1790
|
-
const x = ax + dx * amount;
|
|
1791
|
-
const y = ay + dy * amount;
|
|
1792
|
-
const distance = centerline.startDistance[segmentId] +
|
|
1793
|
-
centerline.distanceSpan[segmentId] * amount;
|
|
1794
|
-
const heading = centerline.heading[segmentId];
|
|
1795
|
-
const normalX = centerline.normalX[segmentId];
|
|
1796
|
-
const normalY = centerline.normalY[segmentId];
|
|
1797
|
-
const px = position.x - x;
|
|
1798
|
-
const py = position.y - y;
|
|
1799
|
-
const signedOffset = px * normalX + py * normalY;
|
|
1800
|
-
|
|
1801
|
-
return {
|
|
1802
|
-
segmentId,
|
|
1803
|
-
x,
|
|
1804
|
-
y,
|
|
1805
|
-
distance,
|
|
1806
|
-
heading,
|
|
1807
|
-
normalX,
|
|
1808
|
-
normalY,
|
|
1809
|
-
curvature: centerline.curvature[segmentId],
|
|
1810
|
-
signedOffset,
|
|
1811
|
-
crossTrackError: Math.abs(signedOffset),
|
|
1812
|
-
distanceSquared: px * px + py * py,
|
|
1813
|
-
};
|
|
1814
|
-
}
|
|
1815
|
-
|
|
1816
|
-
function segmentFromIndex(centerline, segmentId) {
|
|
1817
|
-
if (centerline?.stats) centerline.stats.raySegmentObjectAllocations += 1;
|
|
1818
|
-
return {
|
|
1819
|
-
segmentId,
|
|
1820
|
-
startX: centerline.startX[segmentId],
|
|
1821
|
-
startY: centerline.startY[segmentId],
|
|
1822
|
-
endX: centerline.endX[segmentId],
|
|
1823
|
-
endY: centerline.endY[segmentId],
|
|
1824
|
-
startDistance: centerline.startDistance[segmentId],
|
|
1825
|
-
endDistance: centerline.endDistance[segmentId],
|
|
1826
|
-
normalX: centerline.normalX[segmentId],
|
|
1827
|
-
normalY: centerline.normalY[segmentId],
|
|
1828
|
-
endNormalX: centerline.endNormalX[segmentId],
|
|
1829
|
-
endNormalY: centerline.endNormalY[segmentId],
|
|
1830
|
-
};
|
|
1831
|
-
}
|
|
1832
|
-
|
|
1833
|
-
function seedRayTraceFromNearbyCache(
|
|
1834
|
-
index,
|
|
1835
|
-
grid,
|
|
1836
|
-
origin,
|
|
1837
|
-
vector,
|
|
1838
|
-
radius,
|
|
1839
|
-
maxDistance,
|
|
1840
|
-
segmentMarks,
|
|
1841
|
-
segmentEpoch,
|
|
1842
|
-
cellMarks,
|
|
1843
|
-
cellEpoch,
|
|
1844
|
-
ids,
|
|
1845
|
-
) {
|
|
1846
|
-
const scratch = ensureQueryScratch(index);
|
|
1847
|
-
const traceCache = scratch.rayTraceCache;
|
|
1848
|
-
const originCell = gridCellForPoint(grid, origin, false);
|
|
1849
|
-
if (!originCell) return null;
|
|
1850
|
-
const bucket = rayTraceBucket(vector);
|
|
1851
|
-
const base = rayTraceBaseKey(grid, originCell, radius);
|
|
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
|
-
}
|
|
1868
|
-
if (!seed) return { base, bucket };
|
|
1869
|
-
|
|
1870
|
-
for (const segmentId of seed.segmentIds) {
|
|
1871
|
-
if (segmentMarks[segmentId] === segmentEpoch) continue;
|
|
1872
|
-
segmentMarks[segmentId] = segmentEpoch;
|
|
1873
|
-
ids.push(segmentId);
|
|
1874
|
-
}
|
|
1875
|
-
for (const cellId of seed.cellIds) {
|
|
1876
|
-
cellMarks[cellId] = cellEpoch;
|
|
1877
|
-
}
|
|
1878
|
-
return { base, bucket };
|
|
1879
|
-
}
|
|
1880
|
-
|
|
1881
|
-
function storeRayTraceCache(index, grid, origin, vector, radius, maxDistance, segmentIds, cellIds, seed) {
|
|
1882
|
-
const scratch = ensureQueryScratch(index);
|
|
1883
|
-
const traceCache = scratch.rayTraceCache;
|
|
1884
|
-
const originCell = gridCellForPoint(grid, origin, false);
|
|
1885
|
-
if (!originCell) return;
|
|
1886
|
-
const bucket = seed?.bucket ?? rayTraceBucket(vector);
|
|
1887
|
-
const base = seed?.base ?? rayTraceBaseKey(grid, originCell, radius);
|
|
1888
|
-
const key = rayTraceKey(base, bucket);
|
|
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
|
-
}
|
|
1908
|
-
if (traceCache.size > 96) {
|
|
1909
|
-
const oldestKey = traceCache.keys().next().value;
|
|
1910
|
-
if (oldestKey) traceCache.delete(oldestKey);
|
|
1911
|
-
}
|
|
1912
|
-
}
|
|
1913
|
-
|
|
1914
|
-
function rayTraceBaseKey(grid, center, radius) {
|
|
1915
|
-
return `${center.column}:${center.row}:${radius}:${grid.columns}:${grid.rows}`;
|
|
1916
|
-
}
|
|
1917
|
-
|
|
1918
|
-
function rayTraceKey(base, bucket) {
|
|
1919
|
-
return `${base}:${bucket}`;
|
|
1920
|
-
}
|
|
1921
|
-
|
|
1922
|
-
function rayTraceBucket(vector) {
|
|
1923
|
-
const angleDegrees = Math.atan2(vector.y, vector.x) * 180 / Math.PI;
|
|
1924
|
-
return Math.round(angleDegrees / RAY_TRACE_BUCKET_DEGREES);
|
|
1925
|
-
}
|
|
1926
|
-
|
|
1927
|
-
function finitePoint(point) {
|
|
1928
|
-
return Number.isFinite(point?.x) && Number.isFinite(point?.y);
|
|
1929
|
-
}
|
|
1930
|
-
|
|
1931
|
-
function finiteBounds(bounds) {
|
|
1932
|
-
return Number.isFinite(bounds?.minX) &&
|
|
1933
|
-
Number.isFinite(bounds?.maxX) &&
|
|
1934
|
-
Number.isFinite(bounds?.minY) &&
|
|
1935
|
-
Number.isFinite(bounds?.maxY) &&
|
|
1936
|
-
bounds.minX <= bounds.maxX &&
|
|
1937
|
-
bounds.minY <= bounds.maxY;
|
|
1938
|
-
}
|