From 950cd1c4cdc8eaf409842f9ad6421896a6fde0d0 Mon Sep 17 00:00:00 2001
From: que01 <que01@foxmail.com>
Date: Thu, 6 Aug 2026 17:00:51 +0800
Subject: [PATCH] fix(preview): align traffic-aware vehicles to roads

---
 scripts/lib/cesium-preview.js | 57 ++++++++++++++++++++++++++++++-----
 1 file changed, 49 insertions(+), 8 deletions(-)

diff --git a/scripts/lib/cesium-preview.js b/scripts/lib/cesium-preview.js
index f36cc08..4f5bdb6 100644
--- a/scripts/lib/cesium-preview.js
+++ b/scripts/lib/cesium-preview.js
@@ -15,6 +15,11 @@
   const toggleFps = document.getElementById("toggleFps");
   const toggleDiagnostics = document.getElementById("toggleDiagnostics");
   const assetToggles = document.getElementById("assetToggles");
+  // The exported road surface sits at the 0.35m scene anchor plus 0.03m.
+  // Vehicle models have their wheels at local Y=0, so keep them just clear
+  // of the asphalt instead of using the old visibly floating 1.15m height.
+  const VEHICLE_HEIGHT_METERS = 0.40;
+  const ROUTE_LINE_HEIGHT_METERS = 0.42;
   const semanticToggles = document.getElementById("semanticToggles");
   const vehicleSelect = document.getElementById("vehicleSelect");
   const speedControl = document.getElementById("speedControl");
@@ -591,10 +596,11 @@
 
   function addCruiseVehicle(viewer, segment, index, start, speed, signalData, vehicleModelName) {
     const route = prepareRoute(segment, signalData);
-    const positions = createTrafficAwarePositions(viewer, route, start, speed);
+    const trafficMotion = createTrafficAwarePositions(viewer, route, start, speed);
+    const positions = trafficMotion.positions;
     const flat = [];
     for (const coord of segment.coordinates) {
-      flat.push(coord[0], coord[1], 1.05);
+      flat.push(coord[0], coord[1], ROUTE_LINE_HEIGHT_METERS);
     }
     const routeColor = [
       Cesium.Color.CYAN,
@@ -616,7 +622,7 @@
     const vehicle = viewer.entities.add({
       name: "Cruise vehicle " + (index + 1),
       position: positions,
-      orientation: routeOrientationFromPositions(positions),
+      orientation: routeOrientationFromState(route, trafficMotion.state, speed, 0.0),
       model: {
         uri: vehicleModelName,
         // Keep the library's world scale visible. A minimum screen size made
@@ -686,7 +692,6 @@
 
   function createTrafficAwarePositions(viewer, route, start, speed) {
     const state = { distance: 0, lastTime: start.clone() };
-    const scratchTime = new Cesium.JulianDate();
     viewer.clock.onTick.addEventListener((clock) => {
       const elapsed = Cesium.JulianDate.secondsDifference(clock.currentTime, state.lastTime);
       Cesium.JulianDate.clone(clock.currentTime, state.lastTime);
@@ -702,7 +707,10 @@
       }
       state.distance = (state.distance + advance) % route.length;
     });
-    return new Cesium.CallbackProperty((time, result) => routePositionAtDistance(route, state.distance, result), false);
+    return {
+      state,
+      positions: new Cesium.CallbackProperty((time, result) => routePositionAtDistance(route, state.distance, result), false),
+    };
   }
 
   function nextRouteStop(route, distance) {
@@ -727,7 +735,7 @@
     const b = route.coordinates[index];
     const lon = a[0] + (b[0] - a[0]) * t;
     const lat = a[1] + (b[1] - a[1]) * t;
-    return Cesium.Cartesian3.fromDegrees(lon, lat, 1.15, Cesium.Ellipsoid.WGS84, result);
+    return Cesium.Cartesian3.fromDegrees(lon, lat, VEHICLE_HEIGHT_METERS, Cesium.Ellipsoid.WGS84, result);
   }
 
   function routeOrientation(route, start, speed, yawDegrees) {
@@ -776,8 +784,41 @@
     }, false);
   }
 
-  function routeOrientationFromPositions(positions) {
-    return new Cesium.VelocityOrientationProperty(positions);
+  function routeOrientationFromState(route, state, speed, yawDegrees) {
+    const correction = Cesium.Quaternion.fromAxisAngle(
+      Cesium.Cartesian3.UNIT_Z,
+      Cesium.Math.toRadians(yawDegrees)
+    );
+    const current = new Cesium.Cartesian3();
+    const ahead = new Cesium.Cartesian3();
+    const direction = new Cesium.Cartesian3();
+    const up = new Cesium.Cartesian3();
+    const east = new Cesium.Cartesian3();
+    const north = new Cesium.Cartesian3();
+    const hpr = new Cesium.HeadingPitchRoll(0.0, 0.0, 0.0);
+    const base = new Cesium.Quaternion();
+    return new Cesium.CallbackProperty((time, result) => {
+      routePositionAtDistance(route, state.distance, current);
+      // Sample a small distance ahead, rather than using velocity. This keeps
+      // the car aligned while stopped and preserves the model's route heading.
+      const lookAhead = Math.max(0.8, speed * 0.8);
+      routePositionAtDistance(route, (state.distance + lookAhead) % route.length, ahead);
+      Cesium.Cartesian3.subtract(ahead, current, direction);
+      if (Cesium.Cartesian3.magnitudeSquared(direction) < 0.0001) return result;
+      Cesium.Cartesian3.normalize(direction, direction);
+      Cesium.Cartesian3.normalize(current, up);
+      Cesium.Cartesian3.cross(Cesium.Cartesian3.UNIT_Z, up, east);
+      if (Cesium.Cartesian3.magnitudeSquared(east) < 0.0001) {
+        Cesium.Cartesian3.clone(Cesium.Cartesian3.UNIT_X, east);
+      } else {
+        Cesium.Cartesian3.normalize(east, east);
+      }
+      Cesium.Cartesian3.cross(up, east, north);
+      Cesium.Cartesian3.normalize(north, north);
+      hpr.heading = Math.atan2(Cesium.Cartesian3.dot(direction, east), Cesium.Cartesian3.dot(direction, north));
+      Cesium.Transforms.headingPitchRollQuaternion(current, hpr, undefined, undefined, base);
+      return Cesium.Quaternion.multiply(base, correction, result || new Cesium.Quaternion());
+    }, false);
   }
 
   function createChaseFollow(viewer, positionsProvider) {