diff --git a/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx b/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx index 16f487c..2d06dcc 100644 --- a/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx +++ b/app/src/modules/simulation/vehicle/pathCreator/pathCreator.tsx @@ -9,7 +9,11 @@ import React, { } from "react"; import { LineCurve3, MathUtils, Plane, Vector3 } from "three"; import { Vector3Array } from "../../../../types/world/worldTypes"; -import { useActiveSubTool, useToolMode } from "../../../../store/builder/store"; +import { + useActiveSubTool, + useActiveTool, + useToolMode, +} from "../../../../store/builder/store"; import PointHandler from "./pointHandler"; import { getPathPointByPoints } from "./function/getPathPointByPoints"; import PathHandler from "./pathHandler"; @@ -52,11 +56,11 @@ export default function PathCreator() { const [hoveredPoint, setHoveredPoint] = useState(null); const { activeSubTool } = useActiveSubTool(); + const { activeTool } = useActiveTool(); + const POINT_SNAP_THRESHOLD = 0.5; const CAN_POINT_SNAP = true; - useEffect(() => { - - }, [paths]); + useEffect(() => {}, [paths]); const getAllOtherPathPoints = useCallback((): PointData[] => { if (draftPoints.length === 0) return []; return ( @@ -129,9 +133,7 @@ export default function PathCreator() { ]; }); - useEffect(() => { - - }, [paths]); + useEffect(() => {}, [paths]); const getPathPointById = (uuid: any) => { for (const path of paths) { const point = path.pathPoints.find((p) => p.pointId === uuid); @@ -144,9 +146,10 @@ export default function PathCreator() { return undefined; }; - const handleClick = () => { - if (activeSubTool === "free-hand") return; + const handleClick = (e: any) => { + if (activeTool !== "pen") return; if (toolMode === "3D-Delete") return; + if (e.ctrlKey) return; const intersectionPoint = new Vector3(); const pos = raycaster.ray.intersectPlane(plane, intersectionPoint); @@ -154,8 +157,6 @@ export default function PathCreator() { const pointIntersect = raycaster .intersectObjects(scene.children) .find((intersect) => intersect.object.name === "Path-Point"); - - const pathIntersect = raycaster .intersectObjects(scene.children) @@ -265,7 +266,7 @@ export default function PathCreator() { if (snappedPosition && !snappedPoint) { newPoint.position = snappedPosition; } - + if (pointIntersect && !snappedPoint) { const point = getPathPointById(pointIntersect.object.userData.pointId); diff --git a/app/src/modules/simulation/vehicle/pathCreator/pathHandler.tsx b/app/src/modules/simulation/vehicle/pathCreator/pathHandler.tsx index a75c35d..e824435 100644 --- a/app/src/modules/simulation/vehicle/pathCreator/pathHandler.tsx +++ b/app/src/modules/simulation/vehicle/pathCreator/pathHandler.tsx @@ -1,6 +1,6 @@ import { DragControls, Line } from "@react-three/drei"; import React, { useMemo, useState } from "react"; -import { useActiveSubTool, useToolMode } from "../../../../store/builder/store"; +import { useActiveTool, useToolMode } from "../../../../store/builder/store"; import { useThree } from "@react-three/fiber"; import { LineCurve3, Plane, Vector3 } from "three"; import { getPathsByPointId, setPathPosition } from "./function/getPaths"; @@ -49,7 +49,7 @@ export default function PathHandler({ paths?: any; }>({}); const [isHovered, setIsHovered] = useState(false); - const { activeSubTool } = useActiveSubTool(); + const { activeTool } = useActiveTool(); const plane = useMemo(() => new Plane(new Vector3(0, 1, 0), 0), []); const path = useMemo(() => { const [start, end] = points.map((p) => new Vector3(...p.position)); @@ -64,7 +64,7 @@ export default function PathHandler({ } }; const handleDragStart = (points: [PointData, PointData]) => { - if (activeSubTool !== "free-hand") return; + if (activeTool !== "cursor") return; const intersectionPoint = new Vector3(); const hit = raycaster.ray.intersectPlane(plane, intersectionPoint); if (hit) { @@ -106,6 +106,7 @@ export default function PathHandler({ // } // } // }; + const handleDrag = (points: [PointData, PointData]) => { if (isHovered && dragOffset) { const intersectionPoint = new Vector3(); diff --git a/app/src/modules/simulation/vehicle/pathCreator/pointHandler.tsx b/app/src/modules/simulation/vehicle/pathCreator/pointHandler.tsx index 9812a9b..20e2ddf 100644 --- a/app/src/modules/simulation/vehicle/pathCreator/pointHandler.tsx +++ b/app/src/modules/simulation/vehicle/pathCreator/pointHandler.tsx @@ -1,10 +1,11 @@ import { DragControls } from "@react-three/drei"; -import React, { useCallback, useMemo, useState } from "react"; -import { useActiveSubTool, useToolMode } from "../../../../store/builder/store"; +import React, { useCallback, useEffect, useMemo, useState } from "react"; +import { useActiveTool, useToolMode } from "../../../../store/builder/store"; import { Plane, Vector3 } from "three"; import { useThree } from "@react-three/fiber"; import { handleCanvasCursors } from "../../../../utils/mouseUtils/handleCanvasCursors"; import { getPathsByPointId, setPathPosition } from "./function/getPaths"; +import { aStar } from "../structuredPath/functions/aStar"; type PointData = { pointId: string; position: [number, number, number]; @@ -37,20 +38,22 @@ export default function PointHandler({ hoveredPoint, }: PointHandlerProps) { const { toolMode } = useToolMode(); + const { activeTool } = useActiveTool(); const { scene, raycaster } = useThree(); const [isHovered, setIsHovered] = useState(false); const [dragOffset, setDragOffset] = useState(null); const plane = useMemo(() => new Plane(new Vector3(0, 1, 0), 0), []); - const { activeSubTool } = useActiveSubTool(); const [initialPositions, setInitialPositions] = useState<{ paths?: any; }>({}); - const [selectedPoints, setSelectedPoints] = useState(); + const [selectedPoints, setSelectedPoints] = useState([]); + const POINT_SNAP_THRESHOLD = 0.5; // Distance threshold for snapping in meters const CAN_POINT_SNAP = true; const CAN_ANGLE_SNAP = true; const ANGLE_SNAP_DISTANCE_THRESHOLD = 0.5; + const removePathByPoint = (pointId: string): PathDataInterface[] => { const removedPaths: PathDataInterface[] = []; @@ -183,7 +186,9 @@ export default function PointHandler({ }, [getAllOtherPathPoints] ); - + useEffect(() => { + console.log("selectedPoints: ", selectedPoints); + }, [selectedPoints]); // const setPathPosition = useCallback( // ( // pointUuid: string, @@ -211,13 +216,14 @@ export default function PointHandler({ // return paths.filter((a) => a.pathPoints.some((p) => p.pointId === pointId)); // }; - const handlePointClick = (pointId: string) => { + const handlePointClick = (e: any, pointId: string) => { if (toolMode === "3D-Delete") { removePathByPoint(pointId); + } else if (e.ctrlKey) { } }; const handleDragStart = (point: PointData) => { - if (activeSubTool !== "free-hand") return; + if (activeTool !== "cursor") return; const intersectionPoint = new Vector3(); const hit = raycaster.ray.intersectPlane(plane, intersectionPoint); if (hit) { @@ -271,13 +277,13 @@ export default function PointHandler({ userData={point} onClick={(e) => { e.stopPropagation(); - handlePointClick(point.pointId); + handlePointClick(e, point.pointId); }} onPointerOver={(e) => { if (!hoveredPoint && e.buttons === 0 && !e.ctrlKey) { setHoveredPoint(point); setIsHovered(true); - // handleCanvasCursors("default"); + // handleCanvasCursors("default"); } }} onPointerOut={() => { diff --git a/app/src/modules/simulation/vehicle/structuredPath/functions/aStar.ts b/app/src/modules/simulation/vehicle/structuredPath/functions/aStar.ts new file mode 100644 index 0000000..deeb854 --- /dev/null +++ b/app/src/modules/simulation/vehicle/structuredPath/functions/aStar.ts @@ -0,0 +1,213 @@ +type PointData = { + pointId: string; + position: [number, number, number]; + isCurved?: boolean; + handleA?: [number, number, number] | null; + handleB?: [number, number, number] | null; +}; +interface PathDataInterface { + pathId: string; + isActive?: boolean; + isCurved?: boolean; + pathPoints: [PointData, PointData]; +} + +type PathData = PathDataInterface[]; +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 function aStar( + start: string, + end: string, + points: PointData[], + paths: PathData +) { + // Map points by id for quick access + const pointMap = new Map(points.map((p) => [p.pointId, p])); + + // Build adjacency list from paths + const graph = new Map(); + paths.forEach((path) => { + const pathPoints = path.pathPoints; + for (let i = 0; i < pathPoints.length - 1; i++) { + const a = pathPoints[i].pointId; + const b = pathPoints[i + 1].pointId; + + if (!graph.has(a)) graph.set(a, []); + if (!graph.has(b)) graph.set(b, []); + + graph.get(a)!.push(b); + graph.get(b)!.push(a); + } + }); + + // Manhattan distance heuristic (you can use Euclidean instead) + const heuristic = (a: string, b: string) => { + const pa = pointMap.get(a)!.position; + const pb = pointMap.get(b)!.position; + return ( + Math.abs(pa[0] - pb[0]) + + Math.abs(pa[1] - pb[1]) + + Math.abs(pa[2] - pb[2]) + ); + }; + + const openSet = new Set([start]); + const cameFrom = new Map(); + const gScore = new Map(points.map((p) => [p.pointId, Infinity])); + const fScore = new Map(points.map((p) => [p.pointId, Infinity])); + + gScore.set(start, 0); + fScore.set(start, heuristic(start, end)); + + while (openSet.size > 0) { + // get node in openSet with lowest fScore + let current = [...openSet].reduce((a, b) => + fScore.get(a)! < fScore.get(b)! ? a : b + ); + + if (current === end) { + // reconstruct path + const path: string[] = []; + while (cameFrom.has(current)) { + path.unshift(current); + current = cameFrom.get(current)!; + } + path.unshift(start); + return path; + } + + openSet.delete(current); + + for (const neighbor of graph.get(current) || []) { + const tentativeG = gScore.get(current)! + heuristic(current, neighbor); + if (tentativeG < gScore.get(neighbor)!) { + cameFrom.set(neighbor, current); + gScore.set(neighbor, tentativeG); + fScore.set(neighbor, tentativeG + heuristic(neighbor, end)); + if (!openSet.has(neighbor)) openSet.add(neighbor); + } + } + } + + return null; // no path +}