diff --git a/app/src/modules/simulation/armbot/ArmBot.tsx b/app/src/modules/simulation/armbot/ArmBot.tsx index 1c7bb83..5d55791 100644 --- a/app/src/modules/simulation/armbot/ArmBot.tsx +++ b/app/src/modules/simulation/armbot/ArmBot.tsx @@ -4,6 +4,7 @@ import useModuleStore from "../../../store/useModuleStore"; import { useSimulationStates } from "../../../store/store"; import * as SimulationTypes from '../../../types/simulationTypes'; import { ArmbotInstances } from "./ArmBotInstances"; +import { useResetButtonStore } from "../../../store/usePlayButtonStore"; interface ArmBotState { uuid: string; @@ -38,6 +39,7 @@ const ArmBot = ({ armBots, setArmBots, setStaticMachines }: ArmBotProps) => { const { activeModule } = useModuleStore(); const { scene } = useThree(); const { simulationStates } = useSimulationStates(); + const { isReset } = useResetButtonStore(); useEffect(() => { const filtered = simulationStates.filter((s): s is SimulationTypes.ArmBotEventsSchema => s.type === "ArmBot"); @@ -54,7 +56,7 @@ const ArmBot = ({ armBots, setArmBots, setStaticMachines }: ArmBotProps) => { connections: bot.points.connections })); setArmBots(initialStates); - }, [simulationStates]); + }, [simulationStates, isReset]); useEffect(() => { armBots.forEach((bot) => { diff --git a/app/src/modules/simulation/armbot/IKAnimationController.tsx b/app/src/modules/simulation/armbot/IKAnimationController.tsx index dbb7386..efe88d4 100644 --- a/app/src/modules/simulation/armbot/IKAnimationController.tsx +++ b/app/src/modules/simulation/armbot/IKAnimationController.tsx @@ -31,7 +31,7 @@ interface ArmBotState { type IKAnimationControllerProps = { ikSolver: any; - process: { + processes: { triggerId: string; startPoint: THREE.Vector3; endPoint: THREE.Vector3; @@ -50,7 +50,7 @@ type IKAnimationControllerProps = { const IKAnimationController = ({ ikSolver, - process, + processes, selectedTrigger, targetBoneName, uuid, @@ -67,20 +67,10 @@ const IKAnimationController = ({ const [isInitializing, setIsInitializing] = useState(true); const restSpeed = 0.1; const restPosition = new THREE.Vector3(0, 2, 1.6); - const { isPlaying } = usePlayButtonStore(); + const { isPlaying } = usePlayButtonStore();; + const statusRef = useRef("idle"); const { simulationStates } = useSimulationStates(); - // Track previous states for comparison - const prevStateRef = useRef({ - isInitializing: true, - needsInitialMovement: true, - selectedTrigger: "", - progress: 0 - }); - - // Track previous status for comparison - const prevStatusRef = useRef(""); - const initialCurveRef = useRef(null); const initialStartPositionRef = useRef(null); @@ -101,91 +91,6 @@ const IKAnimationController = ({ } }, [ikSolver]); - // Log state changes - useEffect(() => { - const prev = prevStateRef.current; - - if (prev.isInitializing !== isInitializing) { - if (!isInitializing) { - logStatus(`[Arm ${uuid}] Completed initialization, now at rest position`); - } - } - - if (prev.needsInitialMovement !== needsInitialMovement && !needsInitialMovement) { - logStatus(`[Arm ${uuid}] Reached rest position, ready for animation`); - - } - - if (prev.selectedTrigger !== selectedTrigger) { - logStatus(`[Arm ${uuid}] Processing new trigger: ${selectedTrigger}`); - - const currentProcess = process.find(p => p.triggerId === prev.selectedTrigger); - if (currentProcess) { - const triggerId = currentProcess.triggerId; - - const endPoint = armBot.actions.processes.find((process) => process.triggerId === triggerId)?.endPoint; - - // Search simulationStates for a StaticMachine or Conveyor that has a point matching this endPointId - const matchedMachine = simulationStates.find((state) => { - if (state.type === "Conveyor") { - // For Conveyor, points is an array - return (state).points.some( - (point) => point.uuid === endPoint - ); - } else if (state.type === "StaticMachine") { - // For StaticMachine, points is an object - return state.points.uuid === endPoint; - } - return false; - }); - - if (matchedMachine) { - // Log if the end point is a conveyor - if (matchedMachine.type === "Conveyor") { - logStatus(`[Arm ${uuid}] Reached end point which is a conveyor (${matchedMachine.modelName})`); - } else { - logStatus(`[Arm ${uuid}] Reached end point which is a static machine (${matchedMachine.modelName})`); - } - - if (matchedMachine.type === "StaticMachine") { - setStaticMachines((machines) => { - return machines.map((machine) => { - if (machine.uuid === matchedMachine.modeluuid) { - return { ...machine, status: "running" }; - } else { - return machine; - } - }); - }); - } - - if (matchedMachine.type === "Conveyor") { - setArmBots((prev) => - prev.map((arm) => { - if (arm.uuid === uuid) { - return { - ...arm, - isActive: false - }; - } - else { - return arm; - } - }) - ); - } - } - } - } - - // Update previous state - prevStateRef.current = { - isInitializing, - needsInitialMovement, - selectedTrigger, - progress - }; - }, [isInitializing, needsInitialMovement, selectedTrigger, progress]); const calculateInitialCurve = (startPosition: THREE.Vector3) => { const direction = new THREE.Vector3().subVectors(restPosition, startPosition); @@ -211,61 +116,56 @@ const IKAnimationController = ({ ]); }; - const processedCurves = useMemo(() => { - if (isPlaying) - return process.map((p) => { - const tempLift = 0.5; - const localStart = groupRef.current?.worldToLocal(p.startPoint.clone().add(new THREE.Vector3(0, tempLift, 0))); - const localEnd = groupRef.current?.worldToLocal(p.endPoint.clone().add(new THREE.Vector3(0, tempLift, 0))); + const processCurves = useMemo(() => { + if (!isPlaying) return []; - if (localStart && localEnd) { + return processes.map(process => { + const localStart = groupRef.current?.worldToLocal(process.startPoint.clone()); + const localEnd = groupRef.current?.worldToLocal(process.endPoint.clone()); - const mid = new THREE.Vector3( - (localStart.x + localEnd.x) / 1, - Math.max(localStart.y, localEnd.y) + 0.8, - (localStart.z + localEnd.z) / 0.9 - ); + if (!localStart || !localEnd) return null; - const points = [ - restPosition.clone(), - localStart.clone(), - mid.clone(), - localEnd.clone(), - restPosition.clone(), - ]; - const curve = new THREE.CatmullRomCurve3(points); - const restToStartDist = points[0].distanceTo(points[1]); - const startToEndDist = points[1].distanceTo(points[3]); - const endToRestDist = points[3].distanceTo(points[4]); + const midPoint = new THREE.Vector3( + (localStart.x + localEnd.x) / 2, + Math.max(localStart.y, localEnd.y) + 1, + (localStart.z + localEnd.z) / 2 + ); + const restToStartCurve = new THREE.CatmullRomCurve3([ + restPosition, + new THREE.Vector3().lerpVectors(restPosition, localStart, 0.5), + localStart + ]); - const totalDist = restToStartDist + startToEndDist + endToRestDist; - const restToStartRange = [0, restToStartDist / totalDist]; - const startToEndRange = [ - restToStartRange[1], - restToStartRange[1] + startToEndDist / totalDist, - ]; - const endToRestRange = [startToEndRange[1], 1]; + const processCurve = new THREE.CatmullRomCurve3([ + localStart, + midPoint, + localEnd + ]); - return { - trigger: p.triggerId, - curve, - speed: p.speed, - restToStartRange, - startToEndRange, - endToRestRange, - }; - } - }); - }, [process, groupRef, isPlaying]); + const endToRestCurve = new THREE.CatmullRomCurve3([ + localEnd, + new THREE.Vector3().lerpVectors(localEnd, restPosition, 0.5), + restPosition + ]); - useEffect(() => { - console.log('processedCurves: ', processedCurves); - }, [processedCurves]) + return { + triggerId: process.triggerId, + restToStartCurve, + processCurve, + endToRestCurve, + speed: process.speed, + totalDistance: + restPosition.distanceTo(localStart) + + localStart.distanceTo(localEnd) + + localEnd.distanceTo(restPosition) + }; + }).filter(Boolean); + }, [processes, isPlaying]); - const activeCurve = useMemo(() => { - if (isPlaying && processedCurves) - return processedCurves.find((c) => c?.trigger === selectedTrigger); - }, [processedCurves, selectedTrigger, isPlaying]); + const activeProcess = useMemo(() => { + if (!selectedTrigger) return null; + return processCurves.find(p => p?.triggerId === selectedTrigger); + }, [processCurves, selectedTrigger]); // Initial movement to rest position useFrame((_, delta) => { @@ -293,68 +193,128 @@ const IKAnimationController = ({ // Main animation loop useFrame((_, delta) => { - if (!ikSolver || !activeCurve || isInitializing || !isPlaying) return; + if (isInitializing || !isPlaying || !selectedTrigger || !activeProcess || !ikSolver) return; - const { curve, speed, restToStartRange, startToEndRange, endToRestRange } = activeCurve; - const targetBone = ikSolver.mesh.skeleton.bones.find( - (b: any) => b.name === targetBoneName - ); - if (!targetBone) return; + const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBoneName); + if (!bone) return; - let currentSpeed = restSpeed; - let currentStatus = "idle"; // Default status + const { + restToStartCurve, + processCurve, + endToRestCurve, + speed, + totalDistance + } = activeProcess; - // Determine current phase and status - if (progress < restToStartRange[1]) { - currentSpeed = restSpeed; - currentStatus = "moving"; // Moving to start point - } else if (progress >= startToEndRange[0] && progress < startToEndRange[1]) { - currentSpeed = speed; - currentStatus = "moving"; // Moving between points - } else if (progress >= endToRestRange[0] && progress < 1) { - currentSpeed = restSpeed; - currentStatus = "moving"; // Returning to rest - } else if (progress >= 1) { - currentStatus = "idle"; // Completed cycle - } + // Calculate current segment and progress + const restToStartDist = restPosition.distanceTo(restToStartCurve.points[2]); + const processDist = processCurve.getLength(); + const endToRestDist = endToRestCurve.getLength(); - // Update status when it changes - if (prevStatusRef.current !== currentStatus) { - updateArmBotStatus(currentStatus); - prevStatusRef.current = currentStatus; - } + const restToStartEnd = restToStartDist / totalDistance; + const processEnd = (restToStartDist + processDist) / totalDistance; - // Only update progress if we're not already at the end - if (progress < 1) { - setProgress((prev) => { - const next = prev + delta * currentSpeed; - return Math.min(next, 1); // Cap at 1 - }); - } + setProgress(prev => { + let currentStatus = statusRef.current; + let currentPosition: THREE.Vector3; + const newProgress = Math.min(prev + delta * ((currentStatus === 'returning to rest') ? restSpeed : speed), 1); - // Update bone position based on progress - if (progress < 1) { - targetBone.position.copy(curve.getPoint(progress)); - } else { - targetBone.position.copy(curve.getPoint(1)); - } + if (newProgress < restToStartEnd) { + // Moving from rest to start position + currentStatus = "moving to start"; + const segmentProgress = newProgress / restToStartEnd; + currentPosition = restToStartCurve.getPoint(segmentProgress); + } else if (newProgress < processEnd) { + // Processing - moving from start to end + currentStatus = "processing"; + const segmentProgress = (newProgress - restToStartEnd) / (processEnd - restToStartEnd); + currentPosition = processCurve.getPoint(segmentProgress); + } else { + // Returning to rest position + currentStatus = "returning to rest"; + const segmentProgress = (newProgress - processEnd) / (1 - processEnd); + currentPosition = endToRestCurve.getPoint(segmentProgress); + } - ikSolver.update(); + // Update status if changed + if (currentStatus !== statusRef.current) { + statusRef.current = currentStatus; + // updateArmBotStatus(currentStatus); + logStatus(`[Arm ${uuid}] Status: ${currentStatus}`); + } + + // Only trigger when the entire animation is complete (newProgress === 1) + if (newProgress === 1 && currentStatus === "returning to rest") { + updateConveyorOrStaticMachineStatus(selectedTrigger); + } + + bone.position.copy(currentPosition); + ikSolver.update(); + return newProgress; + }); }); - return ( - <> - {armBot.status === 'moving' && activeCurve && - + const updateConveyorOrStaticMachineStatus = (selectedTrigger: string) => { + const currentProcess = processes.find(p => p.triggerId === selectedTrigger); + if (currentProcess) { + const triggerId = currentProcess.triggerId; + + const endPoint = armBot.actions.processes.find((process) => process.triggerId === triggerId)?.endPoint; + + const matchedMachine = simulationStates.find((state) => { + if (state.type === "Conveyor") { + return (state).points.some( + (point) => point.uuid === endPoint + ); + } else if (state.type === "StaticMachine") { + return state.points.uuid === endPoint; + } + return false; + }); + + if (matchedMachine) { + if (matchedMachine.type === "Conveyor") { + logStatus(`[Arm ${uuid}] Reached end point which is a conveyor (${matchedMachine.modelName})`); + } else { + logStatus(`[Arm ${uuid}] Reached end point which is a static machine (${matchedMachine.modelName})`); + } + + setTimeout(() => { + if (matchedMachine.type === "StaticMachine") { + setStaticMachines((machines) => { + return machines.map((machine) => { + if (machine.uuid === matchedMachine.modeluuid) { + return { ...machine, status: "running" }; + } else { + return machine; + } + }); + }); + updateArmBotStatus('idle'); + } + + if (matchedMachine.type === "Conveyor") { + setArmBots((prev) => + prev.map((arm) => { + if (arm.uuid === uuid) { + return { + ...arm, + isActive: false, + status: "idle", + }; + } + else { + return arm; + } + }) + ); + } + }, 0); } - - ); + } + } + + return null; }; export default IKAnimationController; \ No newline at end of file diff --git a/app/src/modules/simulation/armbot/IkInstances.tsx b/app/src/modules/simulation/armbot/IkInstances.tsx index 5a0d23b..6e7dc13 100644 --- a/app/src/modules/simulation/armbot/IkInstances.tsx +++ b/app/src/modules/simulation/armbot/IkInstances.tsx @@ -132,7 +132,7 @@ const IkInstances = ({ { const filtered = simulationStates.filter((s): s is SimulationTypes.StaticMachineEventsSchema => s.type === "StaticMachine"); @@ -47,7 +49,7 @@ function StaticMachine({ setArmBots, staticMachines, setStaticMachines }: Static connectedArmBot: machine.points.connections.targets[0].modelUUID })); setStaticMachines(initialStates); - }, [simulationStates]); + }, [simulationStates, isReset]); const updateArmBotTriggerAndMachineStatus = (armBotUuid: string, triggerId: string, machineId: string) => { setArmBots((prevArmBots) => {