diff --git a/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx b/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx index 2d06dcc..1722fe9 100644 --- a/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx +++ b/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx @@ -57,7 +57,7 @@ export default function PathCreator() { const { activeSubTool } = useActiveSubTool(); const { activeTool } = useActiveTool(); - + const [pathPointsList, setPathPointsList] = useState([]); const POINT_SNAP_THRESHOLD = 0.5; const CAN_POINT_SNAP = true; useEffect(() => {}, [paths]); @@ -275,7 +275,11 @@ export default function PathCreator() { newPoint.position = point.position; } } - + setPathPointsList((prev) => { + if (!prev.find((p) => p.pointId === newPoint.pointId)) + return [...prev, newPoint]; + return prev; + }); if (draftPoints.length === 0) { setDraftPoints([newPoint]); } else { @@ -329,6 +333,9 @@ export default function PathCreator() { return points; }, [paths]); + useEffect(() => { + console.log("allPoints: ", allPoints); + }, [paths]); return ( <> {/* Draft points (red) */} @@ -340,8 +347,10 @@ export default function PathCreator() { ))} {/* Saved points */} - {allPoints.map((point) => ( + {allPoints.map((point: PointData, i: any) => ( >; hoveredLine: PathDataInterface | null; + pointIndex: any; + points: PointData[]; }; +function dist(a: PointData, b: PointData): number { + return Math.sqrt( + (a.position[0] - b.position[0]) ** 2 + + (a.position[1] - b.position[1]) ** 2 + + (a.position[2] - b.position[2]) ** 2 + ); +} + +/** --- A* Algorithm --- */ +type AStarResult = { + pointIds: string[]; + distance: number; +}; + +function aStarShortestPath( + startId: string, + goalId: string, + points: PointData[], + paths: PathData +): AStarResult | null { + const pointById = new Map(points.map((p) => [p.pointId, p])); + const start = pointById.get(startId); + const goal = pointById.get(goalId); + if (!start || !goal) return null; + + const openSet = new Set([startId]); + const cameFrom: Record = {}; + const gScore: Record = {}; + const fScore: Record = {}; + + for (const p of points) { + cameFrom[p.pointId] = null; + gScore[p.pointId] = Infinity; + fScore[p.pointId] = Infinity; + } + + gScore[startId] = 0; + fScore[startId] = dist(start, goal); + + const neighborsOf = (id: string): { id: string; cost: number }[] => { + const me = pointById.get(id)!; + const out: { id: string; cost: number }[] = []; + for (const edge of paths) { + const [a, b] = edge.pathPoints; + if (a.pointId === id) out.push({ id: b.pointId, cost: dist(me, b) }); + else if (b.pointId === id) out.push({ id: a.pointId, cost: dist(me, a) }); + } + return out; + }; + + while (openSet.size > 0) { + let current: string = [...openSet].reduce((a, b) => + fScore[a] < fScore[b] ? a : b + ); + + if (current === goalId) { + const ids: string[] = []; + let node: string | null = current; + while (node) { + ids.unshift(node); + node = cameFrom[node]; + } + return { pointIds: ids, distance: gScore[goalId] }; + } + + openSet.delete(current); + + for (const nb of neighborsOf(current)) { + const tentativeG = gScore[current] + nb.cost; + if (tentativeG < gScore[nb.id]) { + cameFrom[nb.id] = current; + gScore[nb.id] = tentativeG; + fScore[nb.id] = tentativeG + dist(pointById.get(nb.id)!, goal); + openSet.add(nb.id); + } + } + } + + return null; +} + +/** --- Convert node path to edges --- */ +function nodePathToEdges( + pointIds: string[], + points: PointData[], + paths: PathData +): PathData { + const byId = new Map(points.map((p) => [p.pointId, p])); + const edges: PathData = []; + + for (let i = 0; i < pointIds.length - 1; i++) { + const a = pointIds[i]; + const b = pointIds[i + 1]; + + const edge = paths.find( + (p) => + (p.pathPoints[0].pointId === a && p.pathPoints[1].pointId === b) || + (p.pathPoints[0].pointId === b && p.pathPoints[1].pointId === a) + ); + + if (edge) { + const [p1, p2] = edge.pathPoints; + edges.push({ + pathId: edge.pathId, + pathPoints: + p1.pointId === a + ? ([p1, p2] as [PointData, PointData]) + : ([p2, p1] as [PointData, PointData]), + }); + } else { + const pa = byId.get(a)!; + const pb = byId.get(b)!; + edges.push({ + pathId: `synthetic-${a}-${b}`, + pathPoints: [pa, pb], + }); + } + } + + return edges; +} export default function PointHandler({ point, setPaths, @@ -36,6 +160,8 @@ export default function PointHandler({ setHoveredPoint, hoveredLine, hoveredPoint, + pointIndex, + points, }: PointHandlerProps) { const { toolMode } = useToolMode(); const { activeTool } = useActiveTool(); @@ -47,9 +173,11 @@ export default function PointHandler({ paths?: any; }>({}); const [selectedPoints, setSelectedPoints] = useState([]); - + const [shortestEdges, setShortestEdges] = useState([]); const POINT_SNAP_THRESHOLD = 0.5; // Distance threshold for snapping in meters - + const [selectedPointIndices, setSelectedPointIndices] = useState( + [] + ); const CAN_POINT_SNAP = true; const CAN_ANGLE_SNAP = true; const ANGLE_SNAP_DISTANCE_THRESHOLD = 0.5; @@ -70,6 +198,7 @@ export default function PointHandler({ return removedPaths; }; + const getConnectedPoints = (uuid: string): PointData[] => { const connected: PointData[] = []; @@ -85,6 +214,7 @@ export default function PointHandler({ return connected; }; + const snapPathAngle = useCallback( ( newPosition: [number, number, number], @@ -155,6 +285,7 @@ export default function PointHandler({ }, [] ); + const getAllOtherPathPoints = useCallback((): PointData[] => { return ( paths?.flatMap((path) => @@ -186,42 +317,103 @@ export default function PointHandler({ }, [getAllOtherPathPoints] ); - useEffect(() => { - console.log("selectedPoints: ", selectedPoints); - }, [selectedPoints]); - // const setPathPosition = useCallback( - // ( - // pointUuid: string, - // position: [number, number, number], - // setPaths: React.Dispatch> - // ) => { - // setPaths((prevPaths) => - // prevPaths.map((path) => { - // if (path.pathPoints.some((p) => p.pointId === pointUuid)) { - // return { - // ...path, - // pathPoints: path.pathPoints.map((p) => - // p.pointId === pointUuid ? { ...p, position } : p - // ) as [PointData, PointData], // 👈 force back to tuple - // }; + let isHandlingShiftClick = false; + + // const handlePointClick = (e: any, point: PointData) => { + // if (toolMode === "3D-Delete") { + // removePathByPoint(point.pointId); + // } + // if (e.shiftKey) { + // setSelectedPointIndices((prev) => { + // if (prev.length === 0) return [pointIndex]; + // if (prev.length === 1) { + // // defer shortest path calculation + // setTimeout(() => { + // console.log("points: ", points); + // const p1 = points[prev[0]]; + // console.log(' p1: ', p1); + // const p2 = points[pointIndex]; + // console.log('p2: ', p2); + // const result = aStarShortestPath( + // p1.pointId, + // p2.pointId, + // points, + // paths + // ); + // if (result) { + // const edges = nodePathToEdges(result.pointIds, points, paths); + // console.log("edges: ", edges); + // setShortestEdges(edges); + // } else { + // setShortestEdges([]); // } - // return path; - // }) - // ); - // }, - // [setPaths] - // ); - - // const getPathsByPointId = (pointId: any) => { - // return paths.filter((a) => a.pathPoints.some((p) => p.pointId === pointId)); - // }; - - const handlePointClick = (e: any, pointId: string) => { + // }, 0); + // return [prev[0], pointIndex]; + // } + // return [pointIndex]; + // }); + // } + // }; + const handlePointClick = (e: any, point: PointData) => { + e.stopPropagation(); if (toolMode === "3D-Delete") { - removePathByPoint(pointId); - } else if (e.ctrlKey) { + removePathByPoint(point.pointId); + return; + } + + if (e.shiftKey) { + if (isHandlingShiftClick) return; // prevent double-handling + isHandlingShiftClick = true; + + setTimeout(() => { + isHandlingShiftClick = false; + }, 100); // reset the flag after a short delay + + setSelectedPointIndices((prev) => { + // console.log("Clicked point index:", pointIndex); + console.log("Previous selection:", prev); + + if (prev.length === 0) { + console.log("first works"); + return [pointIndex]; + } + + if (prev.length === 1) { + if (prev[0] === pointIndex) { + console.log("Same point selected twice — ignoring"); + return prev; + } + + const p1 = points[prev[0]]; + const p2 = points[pointIndex]; + + console.log("Point 1:", p1); + console.log("Point 2:", p2); + + if (p1 && p2) { + const result = aStarShortestPath( + p1.pointId, + p2.pointId, + points, + paths + ); + + if (result) { + const edges = nodePathToEdges(result.pointIds, points, paths); + setShortestEdges(edges); + } else { + setShortestEdges([]); + } + } + + return [prev[0], pointIndex]; + } + + return [pointIndex]; + }); } }; + const handleDragStart = (point: PointData) => { if (activeTool !== "cursor") return; const intersectionPoint = new Vector3(); @@ -261,6 +453,11 @@ export default function PointHandler({ pathIntersection.forEach((update) => {}); } }; + + useEffect(() => { + console.log("selectedPoints: ", selectedPoints); + }, [selectedPoints]); + return ( <> { - e.stopPropagation(); - handlePointClick(e, point.pointId); + handlePointClick(e, point); }} onPointerOver={(e) => { if (!hoveredPoint && e.buttons === 0 && !e.ctrlKey) { @@ -303,3 +499,30 @@ export default function PointHandler({ ); } + +// const setPathPosition = useCallback( +// ( +// pointUuid: string, +// position: [number, number, number], +// setPaths: React.Dispatch> +// ) => { +// setPaths((prevPaths) => +// prevPaths.map((path) => { +// if (path.pathPoints.some((p) => p.pointId === pointUuid)) { +// return { +// ...path, +// pathPoints: path.pathPoints.map((p) => +// p.pointId === pointUuid ? { ...p, position } : p +// ) as [PointData, PointData], // 👈 force back to tuple +// }; +// } +// return path; +// }) +// ); +// }, +// [setPaths] +// ); + +// const getPathsByPointId = (pointId: any) => { +// return paths.filter((a) => a.pathPoints.some((p) => p.pointId === pointId)); +// }; diff --git a/app/src/modules/simulation/vehicle/structuredPath/pointHandlers.tsx b/app/src/modules/simulation/vehicle/structuredPath/pointHandlers.tsx index 233450a..0ef25e3 100644 --- a/app/src/modules/simulation/vehicle/structuredPath/pointHandlers.tsx +++ b/app/src/modules/simulation/vehicle/structuredPath/pointHandlers.tsx @@ -602,7 +602,7 @@ export default function PointHandlers({ const handleARef = useRef(null); const handleBRef = useRef(null); const lineRef = useRef(null); - const { camera, gl, controls, raycaster } = useThree(); + const { gl, controls, raycaster } = useThree(); const [dragging, setDragging] = useState< null | "main" | "handleA" | "handleB" @@ -620,6 +620,7 @@ export default function PointHandlers({ if (prev.length === 1) { // defer shortest path calculation setTimeout(() => { + console.log('points: ', points); const p1 = points[prev[0]]; const p2 = points[pointIndex]; const result = aStarShortestPath( @@ -630,6 +631,7 @@ export default function PointHandlers({ ); if (result) { const edges = nodePathToEdges(result.pointIds, points, paths); + console.log('edges: ', edges); setShortestEdges(edges); setShortestPath(edges); } else { diff --git a/app/src/modules/simulation/vehicle/structuredPath/structuredPath.tsx b/app/src/modules/simulation/vehicle/structuredPath/structuredPath.tsx index ee82855..ed4df47 100644 --- a/app/src/modules/simulation/vehicle/structuredPath/structuredPath.tsx +++ b/app/src/modules/simulation/vehicle/structuredPath/structuredPath.tsx @@ -51,7 +51,7 @@ export default function StructuredPath() { // --- State Variables --- const [pathPointsList, setPathPointsList] = useState([]); const [allPaths, setAllPaths] = useState([]); - console.log("allPaths: ", allPaths); + const [computedShortestPath, setComputedShortestPath] = useState( [] );