481 lines
13 KiB
TypeScript
481 lines
13 KiB
TypeScript
import React, { useEffect, useState, useRef, useMemo } from "react";
|
|
import * as THREE from "three";
|
|
import { useFrame, useThree } from "@react-three/fiber";
|
|
import { NavMeshQuery } from "@recast-navigation/core";
|
|
import { Line } from "@react-three/drei";
|
|
import {
|
|
useAnimationPlaySpeed,
|
|
usePlayButtonStore,
|
|
} from "../../../store/usePlayButtonStore";
|
|
import { usePlayAgv } from "../../../store/store";
|
|
|
|
interface PathNavigatorProps {
|
|
navMesh: any;
|
|
pathPoints: any;
|
|
id: string;
|
|
speed: number;
|
|
globalSpeed: number;
|
|
bufferTime: number;
|
|
hitCount: number;
|
|
processes: any[];
|
|
agvRef: any;
|
|
MaterialRef: any;
|
|
}
|
|
interface AGVData {
|
|
processId: string;
|
|
vehicleId: string;
|
|
hitCount: number;
|
|
totalHits: number;
|
|
}
|
|
type Phase = "initial" | "toDrop" | "toPickup";
|
|
type MaterialType = "Box" | "Crate";
|
|
export default function PathNavigator({
|
|
navMesh,
|
|
pathPoints,
|
|
id,
|
|
speed,
|
|
globalSpeed,
|
|
bufferTime,
|
|
hitCount,
|
|
processes,
|
|
agvRef,
|
|
MaterialRef,
|
|
}: PathNavigatorProps) {
|
|
const [currentPhase, setCurrentPhase] = useState<Phase>("initial");
|
|
const [path, setPath] = useState<[number, number, number][]>([]);
|
|
const [toPickupPath, setToPickupPath] = useState<[number, number, number][]>(
|
|
[]
|
|
);
|
|
const [pickupDropPath, setPickupDropPath] = useState<
|
|
[number, number, number][]
|
|
>([]);
|
|
const [dropPickupPath, setDropPickupPath] = useState<
|
|
[number, number, number][]
|
|
>([]);
|
|
const [initialPosition, setInitialPosition] = useState<THREE.Vector3 | null>(
|
|
null
|
|
);
|
|
const [initialRotation, setInitialRotation] = useState<THREE.Euler | null>(
|
|
null
|
|
);
|
|
const [boxVisible, setBoxVisible] = useState(false);
|
|
|
|
const distancesRef = useRef<number[]>([]);
|
|
const totalDistanceRef = useRef(0);
|
|
const progressRef = useRef(0);
|
|
const isWaiting = useRef(false);
|
|
const timeoutRef = useRef<NodeJS.Timeout | null>(null);
|
|
const hasStarted = useRef(false);
|
|
const hasReachedPickup = useRef(false);
|
|
|
|
const { scene } = useThree();
|
|
const { isPlaying } = usePlayButtonStore();
|
|
const { PlayAgv, setPlayAgv } = usePlayAgv();
|
|
|
|
const boxRef = useRef<THREE.Mesh | null>(null);
|
|
|
|
const baseMaterials = useMemo(
|
|
() => ({
|
|
Box: new THREE.MeshStandardMaterial({ color: 0x8b4513 }),
|
|
Crate: new THREE.MeshStandardMaterial({ color: 0x00ff00 }),
|
|
Default: new THREE.MeshStandardMaterial({ color: 0xcccccc }),
|
|
}),
|
|
[]
|
|
);
|
|
|
|
useEffect(() => {
|
|
const object = scene.getObjectByProperty("uuid", id);
|
|
if (object) {
|
|
setInitialPosition(object.position.clone());
|
|
setInitialRotation(object.rotation.clone());
|
|
}
|
|
}, [scene, id]);
|
|
|
|
const computePath = (start: any, end: any) => {
|
|
try {
|
|
const navMeshQuery = new NavMeshQuery(navMesh);
|
|
const { path: segmentPath } = navMeshQuery.computePath(start, end);
|
|
return (
|
|
segmentPath?.map(
|
|
({ x, y, z }) => [x, y + 0.1, z] as [number, number, number]
|
|
) || []
|
|
);
|
|
} catch {
|
|
return [];
|
|
}
|
|
};
|
|
|
|
const resetState = () => {
|
|
if (timeoutRef.current) {
|
|
clearTimeout(timeoutRef.current);
|
|
timeoutRef.current = null;
|
|
}
|
|
|
|
setPath([]);
|
|
setCurrentPhase("initial");
|
|
setToPickupPath([]);
|
|
setPickupDropPath([]);
|
|
setDropPickupPath([]);
|
|
setBoxVisible(false);
|
|
distancesRef.current = [];
|
|
totalDistanceRef.current = 0;
|
|
progressRef.current = 0;
|
|
isWaiting.current = false;
|
|
hasStarted.current = false;
|
|
hasReachedPickup.current = false;
|
|
|
|
if (initialPosition && initialRotation) {
|
|
const object = scene.getObjectByProperty("uuid", id);
|
|
if (object) {
|
|
object.position.copy(initialPosition);
|
|
object.rotation.copy(initialRotation);
|
|
}
|
|
}
|
|
};
|
|
|
|
useEffect(() => {
|
|
if (!isPlaying) {
|
|
resetState();
|
|
}
|
|
|
|
if (!navMesh || pathPoints.length < 2) return;
|
|
|
|
const [pickup, drop] = pathPoints.slice(-2);
|
|
|
|
const object = scene.getObjectByProperty("uuid", id);
|
|
if (!object) return;
|
|
|
|
const currentPosition = object.position;
|
|
|
|
const toPickupPath = computePath(currentPosition, pickup);
|
|
const pickupToDropPath = computePath(pickup, drop);
|
|
const dropToPickupPath = computePath(drop, pickup);
|
|
|
|
if (
|
|
toPickupPath.length &&
|
|
pickupToDropPath.length &&
|
|
dropToPickupPath.length
|
|
) {
|
|
setPickupDropPath(pickupToDropPath);
|
|
setDropPickupPath(dropToPickupPath);
|
|
setToPickupPath(toPickupPath);
|
|
setPath(toPickupPath);
|
|
setCurrentPhase("initial");
|
|
}
|
|
}, [navMesh, pathPoints, hitCount, isPlaying, PlayAgv]);
|
|
|
|
useEffect(() => {
|
|
if (path.length < 2) return;
|
|
|
|
let total = 0;
|
|
const segmentDistances = path.slice(0, -1).map((point, i) => {
|
|
const dist = new THREE.Vector3(...point).distanceTo(
|
|
new THREE.Vector3(...path[i + 1])
|
|
);
|
|
total += dist;
|
|
return dist;
|
|
});
|
|
|
|
distancesRef.current = segmentDistances;
|
|
totalDistanceRef.current = total;
|
|
progressRef.current = 0;
|
|
isWaiting.current = false;
|
|
}, [path]);
|
|
|
|
function logAgvStatus(id: string, status: string) {
|
|
// console.log(
|
|
// `AGV ${id}: ${status}`
|
|
|
|
// );
|
|
}
|
|
|
|
function findProcessByTargetModelUUID(processes: any, targetModelUUID: any) {
|
|
for (const process of processes) {
|
|
for (const path of process.paths) {
|
|
for (const point of path.points) {
|
|
if (
|
|
point.connections?.targets?.some(
|
|
(target: any) => target.modelUUID === targetModelUUID
|
|
)
|
|
) {
|
|
return process.id;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
useEffect(() => {
|
|
if (!scene || !boxRef || !processes || !MaterialRef.current) return;
|
|
|
|
const existingObject = scene.getObjectByProperty("uuid", id);
|
|
if (!existingObject) return;
|
|
|
|
if (boxRef.current?.parent) {
|
|
boxRef.current.parent.remove(boxRef.current);
|
|
boxRef.current = null;
|
|
}
|
|
|
|
if (boxVisible) {
|
|
const matchedProcess = findProcessByTargetModelUUID(processes, id);
|
|
let materialType: "Box" | "Crate" | "Default" = "Default";
|
|
|
|
if (matchedProcess) {
|
|
const materialEntry = MaterialRef.current.find((item: any) =>
|
|
item.objects.some((obj: any) => obj.processId === matchedProcess)
|
|
);
|
|
if (materialEntry) {
|
|
materialType = materialEntry.material;
|
|
}
|
|
}
|
|
|
|
const boxGeometry = new THREE.BoxGeometry(0.5, 0.5, 0.5);
|
|
const boxMesh = new THREE.Mesh(boxGeometry, baseMaterials[materialType]);
|
|
boxMesh.position.y = 1;
|
|
boxMesh.name = `box-${id}`;
|
|
existingObject.add(boxMesh);
|
|
boxRef.current = boxMesh;
|
|
}
|
|
|
|
return () => {
|
|
if (boxRef.current?.parent) {
|
|
boxRef.current.parent.remove(boxRef.current);
|
|
}
|
|
};
|
|
}, [processes, MaterialRef, boxVisible, scene, id, baseMaterials]);
|
|
|
|
useFrame((_, delta) => {
|
|
const currentAgv = (agvRef.current || []).find(
|
|
(agv: AGVData) => agv.vehicleId === id
|
|
);
|
|
|
|
if (!scene || !id || !isPlaying) return;
|
|
|
|
const object = scene.getObjectByProperty("uuid", id);
|
|
if (!object) return;
|
|
|
|
if (isPlaying && !hasStarted.current) {
|
|
hasStarted.current = false;
|
|
progressRef.current = 0;
|
|
isWaiting.current = false;
|
|
if (timeoutRef.current) {
|
|
clearTimeout(timeoutRef.current);
|
|
timeoutRef.current = null;
|
|
}
|
|
}
|
|
|
|
const isAgvReady = () => {
|
|
if (!agvRef.current || agvRef.current.length === 0) return false;
|
|
if (!currentAgv) return false;
|
|
|
|
return currentAgv.isActive && hitCount >= currentAgv.maxHitCount;
|
|
};
|
|
|
|
if (isPlaying && !hasStarted.current && toPickupPath.length > 0) {
|
|
setBoxVisible(false);
|
|
const startPoint = new THREE.Vector3(...toPickupPath[0]);
|
|
object.position.copy(startPoint);
|
|
|
|
if (toPickupPath.length > 1) {
|
|
const nextPoint = new THREE.Vector3(...toPickupPath[1]);
|
|
const direction = nextPoint.clone().sub(startPoint).normalize();
|
|
object.rotation.y = Math.atan2(direction.x, direction.z);
|
|
}
|
|
|
|
hasStarted.current = true;
|
|
progressRef.current = 0;
|
|
hasReachedPickup.current = false;
|
|
setToPickupPath(toPickupPath.slice(-1));
|
|
logAgvStatus(id, "Started from station, heading to pickup");
|
|
return;
|
|
}
|
|
|
|
if (isPlaying && currentPhase === "initial" && !hasReachedPickup.current) {
|
|
const reached = moveAlongPath(
|
|
object,
|
|
path,
|
|
distancesRef.current,
|
|
speed,
|
|
delta,
|
|
progressRef
|
|
);
|
|
|
|
if (reached) {
|
|
hasReachedPickup.current = true;
|
|
if (currentAgv) {
|
|
currentAgv.status = "picking";
|
|
}
|
|
logAgvStatus(id, "Reached pickup point, Waiting for material");
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (isPlaying && currentAgv?.isActive && currentPhase === "initial") {
|
|
if (!isAgvReady()) return;
|
|
setTimeout(() => {
|
|
setBoxVisible(true);
|
|
setPath([...pickupDropPath]);
|
|
setCurrentPhase("toDrop");
|
|
progressRef.current = 0;
|
|
logAgvStatus(id, "Started from pickup point, heading to drop point");
|
|
if (currentAgv) {
|
|
currentAgv.status = "toDrop";
|
|
}
|
|
}, 0);
|
|
return;
|
|
}
|
|
|
|
if (isPlaying && currentPhase === "toDrop") {
|
|
const reached = moveAlongPath(
|
|
object,
|
|
path,
|
|
distancesRef.current,
|
|
speed,
|
|
delta,
|
|
progressRef
|
|
);
|
|
|
|
if (reached && !isWaiting.current) {
|
|
isWaiting.current = true;
|
|
logAgvStatus(id, "Reached drop point");
|
|
if (currentAgv) {
|
|
currentAgv.status = "droping";
|
|
currentAgv.hitCount = currentAgv.hitCount--;
|
|
}
|
|
timeoutRef.current = setTimeout(() => {
|
|
setPath([...dropPickupPath]);
|
|
setCurrentPhase("toPickup");
|
|
progressRef.current = 0;
|
|
isWaiting.current = false;
|
|
setBoxVisible(false);
|
|
if (currentAgv) {
|
|
currentAgv.status = "toPickup";
|
|
}
|
|
logAgvStatus(
|
|
id,
|
|
"Started from droping point, heading to pickup point"
|
|
);
|
|
}, bufferTime * 1000);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (isPlaying && currentPhase === "toPickup") {
|
|
const reached = moveAlongPath(
|
|
object,
|
|
path,
|
|
distancesRef.current,
|
|
speed,
|
|
delta,
|
|
progressRef
|
|
);
|
|
|
|
if (reached) {
|
|
if (currentAgv) {
|
|
currentAgv.isActive = false;
|
|
}
|
|
setCurrentPhase("initial");
|
|
if (currentAgv) {
|
|
currentAgv.status = "picking";
|
|
}
|
|
logAgvStatus(id, "Reached pickup point again, cycle complete");
|
|
}
|
|
return;
|
|
}
|
|
|
|
moveAlongPath(
|
|
object,
|
|
path,
|
|
distancesRef.current,
|
|
speed,
|
|
delta,
|
|
progressRef
|
|
);
|
|
});
|
|
|
|
function moveAlongPath(
|
|
object: THREE.Object3D,
|
|
path: [number, number, number][],
|
|
distances: number[],
|
|
speed: number,
|
|
delta: number,
|
|
progressRef: React.MutableRefObject<number>
|
|
): boolean {
|
|
if (path.length < 2) return false;
|
|
|
|
progressRef.current += delta * (speed * globalSpeed);
|
|
let covered = progressRef.current;
|
|
let accumulated = 0;
|
|
let index = 0;
|
|
|
|
for (; index < distances.length; index++) {
|
|
const dist = distances[index];
|
|
if (accumulated + dist >= covered) break;
|
|
accumulated += dist;
|
|
}
|
|
|
|
if (index >= path.length - 1) {
|
|
if (path.length > 1) {
|
|
const lastDirection = new THREE.Vector3(...path[path.length - 1])
|
|
.sub(new THREE.Vector3(...path[path.length - 2]))
|
|
.normalize();
|
|
object.rotation.y = Math.atan2(lastDirection.x, lastDirection.z);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const start = new THREE.Vector3(...path[index]);
|
|
const end = new THREE.Vector3(...path[index + 1]);
|
|
const dist = distances[index];
|
|
|
|
const t = THREE.MathUtils.clamp((covered - accumulated) / dist, 0, 1);
|
|
object.position.copy(start.clone().lerp(end, t));
|
|
|
|
if (dist > 0.1) {
|
|
const targetDirection = end.clone().sub(start).normalize();
|
|
const targetRotationY = Math.atan2(targetDirection.x, targetDirection.z);
|
|
|
|
const rotationSpeed = Math.min(5 * delta, 1);
|
|
object.rotation.y = THREE.MathUtils.lerp(
|
|
object.rotation.y,
|
|
targetRotationY,
|
|
rotationSpeed
|
|
);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
useEffect(() => {
|
|
return () => {
|
|
if (timeoutRef.current) {
|
|
clearTimeout(timeoutRef.current);
|
|
}
|
|
};
|
|
}, []);
|
|
|
|
return (
|
|
<group name="path-navigator-lines">
|
|
{toPickupPath.length > 0 && (
|
|
<Line
|
|
points={toPickupPath}
|
|
color="blue"
|
|
lineWidth={3}
|
|
dashed
|
|
dashSize={0.75}
|
|
dashScale={2}
|
|
/>
|
|
)}
|
|
|
|
{pickupDropPath.length > 0 && (
|
|
<Line points={pickupDropPath} color="red" lineWidth={3} />
|
|
)}
|
|
|
|
{dropPickupPath.length > 0 && (
|
|
<Line points={dropPickupPath} color="red" lineWidth={3} />
|
|
)}
|
|
</group>
|
|
);
|
|
}
|