diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/OtherVehicle.tsx b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/OtherVehicle.tsx index b5a35336..f0224111 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/OtherVehicle.tsx +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/OtherVehicle.tsx @@ -1,11 +1,12 @@ "use client"; -import { useRef, useMemo, useEffect, useState } from "react"; +import React, { useRef, useEffect, useMemo, useState } from "react"; import { useFrame } from "@react-three/fiber"; import * as THREE from "three"; import { FrameData } from "@/types/FrameData"; import { currentOtherVehicle } from "@/config/otherVehicleConfig"; import URDFLoader from "urdf-loader"; +import { getInterpolatedFrame } from "@/utils/getInterpolatedFrame"; interface OtherVehicleProps { id: string; @@ -25,25 +26,22 @@ export default function OtherVehicle({ id, timeline, time }: OtherVehicleProps) useEffect(() => { const loader = new URDFLoader(); - loader.load( modelPath, (robot) => { robot.scale.set(scale[0], scale[1], scale[2]); robot.rotation.set(rotation[0], rotation[1], rotation[2]); robot.position.set(offset[0], offset[1], offset[2]); - robot.traverse((child) => { - if (child instanceof THREE.Mesh && child.material instanceof THREE.MeshStandardMaterial) { + if ( + child instanceof THREE.Mesh && + child.material instanceof THREE.MeshStandardMaterial + ) { child.material = child.material.clone(); child.material.color.set(bodyColor); } }); - - if (vehicleGroup.current) { - vehicleGroup.current.add(robot); - } - + vehicleGroup.current.add(robot); setIsLoaded(true); }, undefined, @@ -55,13 +53,10 @@ export default function OtherVehicle({ id, timeline, time }: OtherVehicleProps) useFrame(() => { if (!ref.current || timeline.length === 0) return; - - const frame = timeline.find((f) => f.time >= time) ?? timeline.at(-1); + const frame = getInterpolatedFrame(timeline, time); if (!frame) return; - targetPosition.set(frame.x, 0, frame.y); ref.current.position.lerp(targetPosition, 0.2); - targetQuaternion.setFromEuler(new THREE.Euler(0, -frame.yaw, 0)); ref.current.quaternion.slerp(targetQuaternion, 0.2); }); diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Pedestrian.tsx b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Pedestrian.tsx index 9706afab..13b3331a 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Pedestrian.tsx +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Pedestrian.tsx @@ -1,11 +1,19 @@ "use client"; -import { useRef, useMemo, useEffect, useState } from "react"; +import React, { useRef, useMemo, useEffect, useState } from "react"; import { useFrame } from "@react-three/fiber"; -import { Mesh, Object3D, MeshStandardMaterial } from "three"; +import { + Mesh, + Object3D, + MeshStandardMaterial, + Vector3, + Quaternion, + Euler, +} from "three"; import { useGLTF } from "@react-three/drei"; import { FrameData } from "@/types/FrameData"; import { currentPedestrian } from "@/config/pedestrianConfig"; +import { getInterpolatedFrame } from "@/utils/getInterpolatedFrame"; interface PedestrianProps { id: string; @@ -15,12 +23,15 @@ interface PedestrianProps { export default function Pedestrian({ id, timeline, time }: PedestrianProps) { const [mounted, setMounted] = useState(false); - const ref = useRef(null); + const { modelPath, scale, rotation, offset, bodyColor } = currentPedestrian; const { scene } = useGLTF(modelPath); const clonedScene = useMemo(() => scene.clone(true), [scene]); + const targetPosition = useMemo(() => new Vector3(), []); + const targetQuaternion = useMemo(() => new Quaternion(), []); + useEffect(() => { setMounted(true); }, []); @@ -39,17 +50,14 @@ export default function Pedestrian({ id, timeline, time }: PedestrianProps) { useFrame(() => { if (!ref.current || timeline.length === 0) return; - - const frame = timeline.find((f) => f.time >= time) ?? timeline.at(-1); + const frame = getInterpolatedFrame(timeline, time); if (!frame) return; - - ref.current.position.set(frame.x, 0, frame.y); - ref.current.rotation.y = -frame.yaw; + targetPosition.set(frame.x, 0, frame.y); + ref.current.position.lerp(targetPosition, 0.2); + targetQuaternion.setFromEuler(new Euler(0, -frame.yaw, 0)); + ref.current.quaternion.slerp(targetQuaternion, 0.2); }); - const hasSpawned = timeline.length > 0 && timeline[0].time <= time; - if (!mounted || !hasSpawned) return null; - return ( } @@ -59,4 +67,4 @@ export default function Pedestrian({ id, timeline, time }: PedestrianProps) { position={offset} /> ); -} \ No newline at end of file +} diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficCone.tsx b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficCone.tsx index 562ac729..e9991414 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficCone.tsx +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficCone.tsx @@ -1,11 +1,19 @@ "use client"; -import { useRef, useMemo, useEffect, useState } from "react"; +import React, { useRef, useMemo, useEffect, useState } from "react"; import { useFrame } from "@react-three/fiber"; -import { Mesh, Object3D, MeshStandardMaterial } from "three"; +import { + Mesh, + Object3D, + MeshStandardMaterial, + Vector3, + Quaternion, + Euler, +} from "three"; import { useGLTF } from "@react-three/drei"; import { FrameData } from "@/types/FrameData"; import { currentTrafficCone } from "@/config/trafficConeConfig"; +import { getInterpolatedFrame } from "@/utils/getInterpolatedFrame"; interface TrafficConeProps { id: string; @@ -15,12 +23,14 @@ interface TrafficConeProps { export default function TrafficCone({ id, timeline, time }: TrafficConeProps) { const [mounted, setMounted] = useState(false); - const ref = useRef(null); const { modelPath, scale, rotation, offset, bodyColor } = currentTrafficCone; const { scene } = useGLTF(modelPath); const clonedScene = useMemo(() => scene.clone(true), [scene]); + const targetPosition = useMemo(() => new Vector3(), []); + const targetQuaternion = useMemo(() => new Quaternion(), []); + useEffect(() => { setMounted(true); }, []); @@ -38,16 +48,15 @@ export default function TrafficCone({ id, timeline, time }: TrafficConeProps) { }, [clonedScene, bodyColor]); useFrame(() => { - const frame = timeline.find((f) => f.time >= time); - if (frame && ref.current) { - ref.current.position.set(frame.x, frame.z, frame.y); - ref.current.rotation.y = -frame.yaw; - } + if (!ref.current || timeline.length === 0) return; + const frame = getInterpolatedFrame(timeline, time); + if (!frame) return; + targetPosition.set(frame.x, frame.z, frame.y); + ref.current.position.lerp(targetPosition, 0.2); + targetQuaternion.setFromEuler(new Euler(0, -frame.yaw, 0)); + ref.current.quaternion.slerp(targetQuaternion, 0.2); }); - const hasSpawned = timeline.length > 0 && timeline[0].time <= time; - if (!mounted || !hasSpawned) return null; - return ( } diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficLight.tsx b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficLight.tsx index 5ccced05..59039ab1 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficLight.tsx +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/TrafficLight.tsx @@ -1,11 +1,19 @@ "use client"; -import { useRef, useMemo, useEffect, useState } from "react"; +import React, { useRef, useMemo, useEffect, useState } from "react"; import { useFrame } from "@react-three/fiber"; -import { Mesh, Object3D, MeshStandardMaterial } from "three"; +import { + Mesh, + Object3D, + MeshStandardMaterial, + Vector3, + Quaternion, + Euler, +} from "three"; import { useGLTF } from "@react-three/drei"; import { FrameData } from "@/types/FrameData"; import { currentTrafficLight } from "@/config/trafficLightConfig"; +import { getInterpolatedFrame } from "@/utils/getInterpolatedFrame"; interface TrafficLightProps { id: string; @@ -13,14 +21,20 @@ interface TrafficLightProps { time: number; } -export default function TrafficLight({ id, timeline, time }: TrafficLightProps) { +export default function TrafficLight({ + id, + timeline, + time, +}: TrafficLightProps) { const [mounted, setMounted] = useState(false); - const ref = useRef(null); const { modelPath, scale, rotation, offset, bodyColor } = currentTrafficLight; const { scene } = useGLTF(modelPath); const clonedScene = useMemo(() => scene.clone(true), [scene]); + const targetPosition = useMemo(() => new Vector3(), []); + const targetQuaternion = useMemo(() => new Quaternion(), []); + useEffect(() => { setMounted(true); }, []); @@ -38,16 +52,15 @@ export default function TrafficLight({ id, timeline, time }: TrafficLightProps) }, [clonedScene, bodyColor]); useFrame(() => { - const frame = timeline.find((f) => f.time >= time); - if (frame && ref.current) { - ref.current.position.set(frame.x, frame.z, frame.y); - ref.current.rotation.y = -frame.yaw; - } + if (!ref.current || timeline.length === 0) return; + const frame = getInterpolatedFrame(timeline, time); + if (!frame) return; + targetPosition.set(frame.x, frame.z, frame.y); + ref.current.position.lerp(targetPosition, 0.2); + targetQuaternion.setFromEuler(new Euler(0, -frame.yaw, 0)); + ref.current.quaternion.slerp(targetQuaternion, 0.2); }); - const hasSpawned = timeline.length > 0 && timeline[0].time <= time; - if (!mounted || !hasSpawned) return null; - return ( } diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Vehicle.tsx b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Vehicle.tsx index a972f4a8..eb4a2452 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Vehicle.tsx +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/components/Vehicle.tsx @@ -6,6 +6,7 @@ import { currentVehicle } from "@/config/vehicleConfig"; import { FrameData } from "@/types/FrameData"; import { OrbitControls } from "@react-three/drei"; import URDFLoader from "urdf-loader"; +import { getInterpolatedFrame } from "@/utils/getInterpolatedFrame"; interface VehicleProps { timeline: FrameData[]; @@ -15,7 +16,7 @@ interface VehicleProps { export default function Vehicle({ timeline, time }: VehicleProps) { const ref = useRef(null); const vehicleGroup = useRef(new THREE.Group()); - const mode = useCameraController(ref, timeline, time); + const mode = useCameraController(ref); const targetPosition = useMemo(() => new THREE.Vector3(), []); const targetQuaternion = useMemo(() => new THREE.Quaternion(), []); @@ -55,14 +56,11 @@ export default function Vehicle({ timeline, time }: VehicleProps) { useFrame(() => { if (!ref.current || timeline.length === 0) return; - - const frame = timeline.find((f) => f.time >= time) ?? timeline.at(-1); - if (!frame) return; - - targetPosition.set(frame.x, 0, frame.y); + const interp = getInterpolatedFrame(timeline, time); + if (!interp) return; + targetPosition.set(interp.x, 0, interp.y); ref.current.position.lerp(targetPosition, 0.2); - - targetQuaternion.setFromEuler(new THREE.Euler(0, -frame.yaw, 0)); + targetQuaternion.setFromEuler(new THREE.Euler(0, -interp.yaw, 0)); ref.current.quaternion.slerp(targetQuaternion, 0.2); }); @@ -74,4 +72,4 @@ export default function Vehicle({ timeline, time }: VehicleProps) { {mode === "free" && } ); -} \ No newline at end of file +} diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/hooks/useCameraController.ts b/GEMstack/onboard/visualization/sr_viz/threeD/src/hooks/useCameraController.ts index 01649c9d..326cef29 100644 --- a/GEMstack/onboard/visualization/sr_viz/threeD/src/hooks/useCameraController.ts +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/hooks/useCameraController.ts @@ -1,15 +1,14 @@ +"use client"; + import { useEffect, useState, useRef } from "react"; import { useThree, useFrame } from "@react-three/fiber"; import * as THREE from "three"; -import { FrameData } from "@/types/FrameData"; import { cameraConfig } from "@/config/cameraConfig"; type CameraModeKey = "chase" | "top" | "side" | "free" | "first"; export function useCameraController( - carRef: React.RefObject, - timeline: FrameData[] = [], - time: number + carRef: React.RefObject ): CameraModeKey { const { camera, gl } = useThree(); const [mode, setMode] = useState("chase"); @@ -19,100 +18,69 @@ export function useCameraController( const smoothedPos = useRef(new THREE.Vector3()); const smoothedLookAt = useRef(new THREE.Vector3()); - useEffect(() => { - modeRef.current = mode; - }, [mode]); + useEffect(() => { modeRef.current = mode; }, [mode]); - // Allow switching between all modes at any time useEffect(() => { const handleKey = (e: KeyboardEvent) => { switch (e.key) { - case "1": - setMode("first"); - break; - case "2": - setMode("chase"); - break; - case "3": - setMode("top"); - break; - case "4": - setMode("side"); - break; - case "5": - lastMode.current = modeRef.current; - setMode("free"); - break; - case "0": - if (lastMode.current !== "free") { - setMode(lastMode.current); - } - break; + case "1": setMode("first"); break; + case "2": setMode("chase"); break; + case "3": setMode("top"); break; + case "4": setMode("side"); break; + case "5": lastMode.current = modeRef.current; setMode("free"); break; + case "0": if (lastMode.current !== "free") setMode(lastMode.current); break; } }; - window.addEventListener("keydown", handleKey); return () => window.removeEventListener("keydown", handleKey); }, []); - // Switch to free mode on scroll or click useEffect(() => { const dom = gl.domElement; - - const activateFreeMode = () => { + const activateFree = () => { if (modeRef.current !== "free") { lastMode.current = modeRef.current; setMode("free"); } }; - - const onMouseDown = (e: MouseEvent) => { - if (e.button === 0) activateFreeMode(); // Left click - }; - - const onWheel = () => { - activateFreeMode(); // Scroll - }; + const onMouseDown = (e: MouseEvent) => { if (e.button === 0) activateFree(); }; + const onWheel = () => activateFree(); dom.addEventListener("mousedown", onMouseDown); dom.addEventListener("wheel", onWheel); - return () => { dom.removeEventListener("mousedown", onMouseDown); dom.removeEventListener("wheel", onWheel); }; }, [gl.domElement]); - useFrame(() => { - if (modeRef.current === "free") return; - if (!carRef.current) return; - - // Always fallback to a stable frame if timeline is empty - const frame = - timeline.find((f) => f.time >= time) ?? - timeline.at(-1) ?? - { x: 0, y: 0, yaw: 0 }; + useFrame((_, delta) => { + if (modeRef.current === "free" || !carRef.current) return; const config = cameraConfig[modeRef.current]; - if (!config?.position || !config?.lookAt) return; + if (!config) return; - const carPos = new THREE.Vector3(frame.x, 0, frame.y); - const carQuat = new THREE.Quaternion().setFromEuler(new THREE.Euler(0, -frame.yaw, 0)); + const alpha = 1 - Math.exp(-10.0 * delta); - const damping = config.damping ?? 0.1; + const carPos = new THREE.Vector3(); + carRef.current.getWorldPosition(carPos); + const carQuat = new THREE.Quaternion(); + carRef.current.getWorldQuaternion(carQuat); - // camera offset (relative to vehicle) and final position - const offset = new THREE.Vector3(...config.position).applyQuaternion(carQuat); - const camTargetPos = carPos.clone().add(offset); - smoothedPos.current.lerp(camTargetPos, damping); + const [px, py, pz] = config.position ?? [0, 0, 0]; + const camTargetPos = carPos.clone().add( + new THREE.Vector3(px, py, pz).applyQuaternion(carQuat) + ); + smoothedPos.current.lerp(camTargetPos, alpha); camera.position.copy(smoothedPos.current); - // look at offset - const lookAtOffset = new THREE.Vector3(...config.lookAt).applyQuaternion(carQuat); - const camLookAt = carPos.clone().add(lookAtOffset); - smoothedLookAt.current.lerp(camLookAt, damping); + const [lx, ly, lz] = config.lookAt ?? [0, 0, 0]; + const camLookAt = carPos.clone().add( + new THREE.Vector3(lx, ly, lz).applyQuaternion(carQuat) + ); + smoothedLookAt.current.lerp(camLookAt, alpha); camera.lookAt(smoothedLookAt.current); }); return mode; -} +} \ No newline at end of file diff --git a/GEMstack/onboard/visualization/sr_viz/threeD/src/utils/getInterpolatedFrame.ts b/GEMstack/onboard/visualization/sr_viz/threeD/src/utils/getInterpolatedFrame.ts new file mode 100644 index 00000000..cc38d019 --- /dev/null +++ b/GEMstack/onboard/visualization/sr_viz/threeD/src/utils/getInterpolatedFrame.ts @@ -0,0 +1,30 @@ +import { FrameData } from "@/types/FrameData"; + +export function getInterpolatedFrame(frames: FrameData[], t: number): FrameData | null { + if (frames.length === 0) return null; + // If before first or after last, clamp + if (t <= frames[0].time) return frames[0]; + if (t >= frames[frames.length - 1].time) return frames[frames.length - 1]; + + // binary search for the first frame with time > t + let lo = 0, hi = frames.length - 1; + while (lo < hi) { + const mid = Math.floor((lo + hi) / 2); + if (frames[mid].time > t) hi = mid; + else lo = mid + 1; + } + const b = frames[lo]; + const a = frames[lo - 1]; + + const α = (t - a.time) / (b.time - a.time); + return { + time: t, + x: a.x * (1 - α) + b.x * α, + y: a.y * (1 - α) + b.y * α, + z: a.z * (1 - α) + b.z * α, + yaw: a.yaw * (1 - α) + b.yaw * α, + pitch: a.pitch * (1 - α) + b.pitch * α, + roll: a.roll * (1 - α) + b.roll * α, + metadata: a.metadata, // or merge if needed + }; +}