diff --git a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx index 06d50a1..ceccd40 100644 --- a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx +++ b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx @@ -1,13 +1,13 @@ -import React, { useEffect, useRef, useState } from "react"; -import { useFrame } from "@react-three/fiber"; -import * as THREE from "three"; -import { Line } from "@react-three/drei"; +import React, { useEffect, useMemo, useRef, useState } from 'react'; +import { useFrame } from '@react-three/fiber'; +import * as THREE from 'three'; +import { Line } from '@react-three/drei'; import { useAnimationPlaySpeed, usePauseButtonStore, usePlayButtonStore, - useResetButtonStore, -} from "../../../../../store/usePlayButtonStore"; + useResetButtonStore +} from '../../../../../store/usePlayButtonStore'; function RoboticArmAnimator({ HandleCallback, @@ -16,24 +16,21 @@ function RoboticArmAnimator({ targetBone, armBot, logStatus, - path, + path }: any) { const progressRef = useRef(0); const curveRef = useRef(null); - const [currentPath, setCurrentPath] = useState<[number, number, number][]>( - [] - ); - const [circlePoints, setCirclePoints] = useState<[number, number, number][]>( - [] - ); - const [customCurvePoints, setCustomCurvePoints] = useState< - THREE.Vector3[] | null - >(null); - + const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]); + const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]); + const [customCurvePoints, setCustomCurvePoints] = useState(null); + let curveHeight = 1.75 + const totalDistanceRef = useRef(0); + const startTimeRef = useRef(null); + const segmentDistancesRef = useRef([]); // Zustand stores const { isPlaying } = usePlayButtonStore(); const { isPaused } = usePauseButtonStore(); - const { isReset } = useResetButtonStore(); + const { isReset, setReset } = useResetButtonStore(); const { speed } = useAnimationPlaySpeed(); // Update path state whenever `path` prop changes @@ -41,20 +38,26 @@ function RoboticArmAnimator({ setCurrentPath(path); }, [path]); - // Reset logic when `isPlaying` changes - useEffect(() => { - if (!isPlaying) { - setCurrentPath([]); - curveRef.current = null; - } - }, [isPlaying]); - // Handle circle points based on armBot position useEffect(() => { - const points = generateRingPoints(1.6, 64); + const points = generateRingPoints(1.6, 64) setCirclePoints(points); }, [armBot.position]); + useEffect(() => { + if (isReset || !isPlaying) { + progressRef.current = 0; + curveRef.current = null; + setCurrentPath([]); + setCustomCurvePoints(null); + totalDistanceRef.current = 0; + startTimeRef.current = null; + segmentDistancesRef.current = []; + setReset(false); + } + }, [isReset, isPlaying]) + + function generateRingPoints(radius: any, segments: any) { const points: [number, number, number][] = []; for (let i = 0; i < segments; i++) { @@ -68,11 +71,7 @@ function RoboticArmAnimator({ return points; } - const findNearestIndex = ( - nearestPoint: [number, number, number], - points: [number, number, number][], - epsilon = 1e-6 - ) => { + const findNearestIndex = (nearestPoint: [number, number, number], points: [number, number, number][], epsilon = 1e-6) => { for (let i = 0; i < points.length; i++) { const [x, y, z] = points[i]; if ( @@ -89,168 +88,150 @@ function RoboticArmAnimator({ // Handle nearest points and final path (including arc points) useEffect(() => { if (circlePoints.length > 0 && currentPath.length > 0) { + const start = currentPath[0]; const end = currentPath[currentPath.length - 1]; - const raisedStart = [start[0], start[1] + 0.5, start[2]] as [ - number, - number, - number - ]; - const raisedEnd = [end[0], end[1] + 0.5, end[2]] as [ - number, - number, - number - ]; + const raisedStart = [start[0], start[1] + 0.5, start[2]] as [number, number, number]; + const raisedEnd = [end[0], end[1] + 0.5, end[2]] as [number, number, number]; const findNearest = (target: [number, number, number]) => { return circlePoints.reduce((nearest, point) => { - const distance = Math.hypot( - target[0] - point[0], - target[1] - point[1], - target[2] - point[2] - ); - const nearestDistance = Math.hypot( - target[0] - nearest[0], - target[1] - nearest[1], - target[2] - nearest[2] - ); + const distance = Math.hypot(target[0] - point[0], target[1] - point[1], target[2] - point[2]); + const nearestDistance = Math.hypot(target[0] - nearest[0], target[1] - nearest[1], target[2] - nearest[2]); return distance < nearestDistance ? point : nearest; }, circlePoints[0]); }; const nearestToStart = findNearest(raisedStart); - const nearestToEnd = findNearest(raisedEnd); - const indexOfNearestStart = findNearestIndex( - nearestToStart, - circlePoints - ); - + const indexOfNearestStart = findNearestIndex(nearestToStart, circlePoints); const indexOfNearestEnd = findNearestIndex(nearestToEnd, circlePoints); - // Find clockwise and counter-clockwise distances - const clockwiseDistance = - (indexOfNearestEnd - indexOfNearestStart + 64) % 64; - - const counterClockwiseDistance = - (indexOfNearestStart - indexOfNearestEnd + 64) % 64; - + const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + 64) % 64; + const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + 64) % 64; const clockwiseIsShorter = clockwiseDistance <= counterClockwiseDistance; - // Collect arc points between start and end let arcPoints: [number, number, number][] = []; if (clockwiseIsShorter) { if (indexOfNearestStart <= indexOfNearestEnd) { - arcPoints = circlePoints.slice( - indexOfNearestStart, - indexOfNearestEnd + 1 - ); + arcPoints = circlePoints.slice(indexOfNearestStart, indexOfNearestEnd + 1); } else { - // Wrap around arcPoints = [ ...circlePoints.slice(indexOfNearestStart, 64), - ...circlePoints.slice(0, indexOfNearestEnd + 1), + ...circlePoints.slice(0, indexOfNearestEnd + 1) ]; } - } else if (indexOfNearestStart >= indexOfNearestEnd) { - for ( - let i = indexOfNearestStart; - i !== (indexOfNearestEnd - 1 + 64) % 64; - i = (i - 1 + 64) % 64 - ) { - arcPoints.push(circlePoints[i]); + } else { + if (indexOfNearestStart >= indexOfNearestEnd) { + for (let i = indexOfNearestStart; i !== (indexOfNearestEnd - 1 + 64) % 64; i = (i - 1 + 64) % 64) { + arcPoints.push(circlePoints[i]); + } + } else { + for (let i = indexOfNearestStart; i !== (indexOfNearestEnd - 1 + 64) % 64; i = (i - 1 + 64) % 64) { + arcPoints.push(circlePoints[i]); + } } } - // Continue your custom path logic const pathVectors = [ - new THREE.Vector3(start[0], start[1], start[2]), // start - new THREE.Vector3(raisedStart[0], raisedStart[1], raisedStart[2]), // lift up - new THREE.Vector3(nearestToStart[0], raisedStart[1], nearestToStart[2]), // move to arc start - ...arcPoints.map( - (point) => new THREE.Vector3(point[0], raisedStart[1], point[2]) - ), - new THREE.Vector3(nearestToEnd[0], raisedEnd[1], nearestToEnd[2]), // move from arc end - new THREE.Vector3(raisedEnd[0], raisedEnd[1], raisedEnd[2]), // lowered end - new THREE.Vector3(end[0], end[1], end[2]), // end + new THREE.Vector3(start[0], start[1], start[2]), + new THREE.Vector3(start[0], curveHeight, start[2]), + new THREE.Vector3(nearestToStart[0], curveHeight, nearestToStart[2]), + ...arcPoints.map(point => new THREE.Vector3(point[0], curveHeight, point[2])), + new THREE.Vector3(nearestToEnd[0], curveHeight, nearestToEnd[2]), + new THREE.Vector3(end[0], curveHeight, end[2]), + new THREE.Vector3(end[0], end[1], end[2]) ]; - const customCurve = new THREE.CatmullRomCurve3( - pathVectors, - false, - "centripetal", - 1 - ); - const generatedPoints = customCurve.getPoints(100); - setCustomCurvePoints(generatedPoints); + + const pathSegments: [THREE.Vector3, THREE.Vector3][] = []; + + for (let i = 0; i < pathVectors.length - 1; i++) { + pathSegments.push([pathVectors[i], pathVectors[i + 1]]); + } + + const segmentDistances = pathSegments.map(([p1, p2]) => p1.distanceTo(p2)); + segmentDistancesRef.current = segmentDistances; + const totalDistance = segmentDistances.reduce((sum, d) => sum + d, 0); + totalDistanceRef.current = totalDistance; + + const movementSpeed = speed * armBot.speed; + const totalMoveTime = totalDistance / movementSpeed; + + const segmentTimes = segmentDistances.map(distance => (distance / totalDistance) * totalMoveTime); + + setCustomCurvePoints(pathVectors); + } }, [circlePoints, currentPath]); // Frame update for animation - useFrame((_, delta) => { + useFrame((state, delta) => { if (!ikSolver) return; - const bone = ikSolver.mesh.skeleton.bones.find( - (b: any) => b.name === targetBone - ); + const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBone); if (!bone) return; if (isPlaying) { - if (!isPaused && customCurvePoints && currentPath.length > 0) { - const curvePoints = customCurvePoints; - const speedAdjustedProgress = - progressRef.current + speed * armBot.speed; - const index = Math.floor(speedAdjustedProgress); + if (!isPaused && customCurvePoints && customCurvePoints.length > 0) { + const distances = segmentDistancesRef.current; // distances between each pair of points + const totalDistance = totalDistanceRef.current; - if (index >= curvePoints.length) { - // Reached the end of the curve + progressRef.current += delta * (speed * armBot.speed); + const coveredDistance = progressRef.current; + + let index = 0; + let accumulatedDistance = 0; + + // Find which segment we are currently in + while (index < distances.length && coveredDistance > accumulatedDistance + distances[index]) { + accumulatedDistance += distances[index]; + index++; + } + if (index < distances.length) { + const startPoint = customCurvePoints[index]; + const endPoint = customCurvePoints[index + 1]; + const segmentDistance = distances[index]; + const t = (coveredDistance - accumulatedDistance) / segmentDistance; + if (startPoint && endPoint) { + const position = startPoint.clone().lerp(endPoint, t); + bone.position.copy(position); + } + } + if (progressRef.current >= totalDistance) { HandleCallback(); setCurrentPath([]); + setCustomCurvePoints([]); curveRef.current = null; progressRef.current = 0; - } else { - const point = curvePoints[index]; - bone.position.copy(point); - progressRef.current = speedAdjustedProgress; + startTimeRef.current = null; } - } else if (isPaused) { - logStatus(armBot.modelUuid, "Simulation Paused"); - } - ikSolver.update(); - } else if (!isPlaying && currentPath.length === 0) { - // Not playing anymore, reset to rest + ikSolver.update(); + } + } else if ((!isPlaying && currentPath.length === 0) || isReset) { bone.position.copy(restPosition); ikSolver.update(); } + }); return ( <> - {customCurvePoints && currentPath && isPlaying && ( + {customCurvePoints && customCurvePoints?.length >= 2 && currentPath && isPlaying && ( [p.x, p.y, p.z] as [number, number, number] - )} + points={customCurvePoints.map((p) => [p.x, p.y, p.z] as [number, number, number])} color="green" lineWidth={5} dashed={false} /> )} - + @@ -258,4 +239,4 @@ function RoboticArmAnimator({ ); } -export default RoboticArmAnimator; +export default RoboticArmAnimator; \ No newline at end of file diff --git a/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx b/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx index 45cae97..60d4c33 100644 --- a/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx +++ b/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx @@ -51,13 +51,12 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { pauseTimeRef.current = null; } const elapsedTime = performance.now() - startTime; - if (elapsedTime < 1500) { + if (elapsedTime < 1000) { // Wait until 1500ms has passed requestAnimationFrame(step); return; } if (currentPhase === "picking") { - setArmBotActive(armBot.modelUuid, true); setArmBotState(armBot.modelUuid, "running"); setCurrentPhase("start-to-end"); @@ -75,7 +74,6 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { } logStatus(armBot.modelUuid, "Moving armBot from start point to end position.") } else if (currentPhase === "dropping") { - setArmBotActive(armBot.modelUuid, true); setArmBotState(armBot.modelUuid, "running"); setCurrentPhase("end-to-rest"); @@ -91,35 +89,26 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { } logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.") } - } useEffect(() => { isPausedRef.current = isPaused; }, [isPaused]); useEffect(() => { - const targetMesh = scene?.getObjectByProperty("uuid", armBot.modelUuid); - - if (targetMesh) { - targetMesh.visible = activeModule !== "simulation" - } - - const targetBones = ikSolver?.mesh.skeleton.bones.find( - (b: any) => b.name === targetBone - ); - if (isReset) { - + if (isReset || !isPlaying) { logStatus(armBot.modelUuid, "Simulation Play Reset Successfully") removeCurrentAction(armBot.modelUuid) - setArmBotActive(armBot.modelUuid, true) - setArmBotState(armBot.modelUuid, "running") - setCurrentPhase("init-to-rest"); + setArmBotActive(armBot.modelUuid, false) + setArmBotState(armBot.modelUuid, "idle") + setCurrentPhase("init"); isPausedRef.current = false pauseTimeRef.current = null - isPausedRef.current = false startTime = 0 + const targetBones = ikSolver?.mesh.skeleton.bones.find( + (b: any) => b.name === targetBone + ); if (targetBones) { - let curve = createCurveBetweenTwoPoints(targetBones.position, targetBones.position) + let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition) if (curve) { setPath(curve.points.map(point => [point.x, point.y, point.z])); } @@ -127,6 +116,16 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { setReset(false); logStatus(armBot.modelUuid, "Moving armBot from initial point to rest position.") } + }, [isReset, isPlaying]) + + useEffect(() => { + const targetMesh = scene?.getObjectByProperty("uuid", armBot.modelUuid); + if (targetMesh) { + targetMesh.visible = activeModule !== "simulation" + } + const targetBones = ikSolver?.mesh.skeleton.bones.find( + (b: any) => b.name === targetBone + ); if (isPlaying) { //Moving armBot from initial point to rest position. @@ -136,7 +135,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { setArmBotState(armBot.modelUuid, "running") setCurrentPhase("init-to-rest"); if (targetBones) { - let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition) + let curve = createCurveBetweenTwoPoints(targetBones.position, targetBones.position) if (curve) { setPath(curve.points.map(point => [point.x, point.y, point.z])); } @@ -157,10 +156,9 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { setArmBotActive(armBot.modelUuid, true); setArmBotState(armBot.modelUuid, "running"); setCurrentPhase("rest-to-start"); - let actiondata = getActionByUuid(selectedProduct.productId, selectedAction.actionId) const startPoint = armBot.point.actions[0].process.startPoint; if (startPoint) { - let curve = createCurveBetweenTwoPoints(restPosition, new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2])); + let curve = createCurveBetweenTwoPoints(targetBones.position, new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2])); if (curve) { setPath(curve.points.map(point => [point.x, point.y, point.z])); } @@ -206,16 +204,19 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { // logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.") } } else { - logStatus(armBot.modelUuid, "Simulation Play Stopped") + logStatus(armBot.modelUuid, "Simulation Play Exited") setArmBotActive(armBot.modelUuid, false) setArmBotState(armBot.modelUuid, "idle") setCurrentPhase("init"); setPath([]) + isPausedRef.current = false + pauseTimeRef.current = null + isPausedRef.current = false + startTime = 0 removeCurrentAction(armBot.modelUuid) - } - }, [currentPhase, armBot, isPlaying, ikSolver, isReset]) + }, [currentPhase, armBot, isPlaying, ikSolver]) function createCurveBetweenTwoPoints(p1: any, p2: any) { @@ -258,6 +259,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { } } const logStatus = (id: string, status: string) => { + // } diff --git a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx index 4a8947b..c090a92 100644 --- a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx +++ b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx @@ -70,7 +70,7 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns setSelectedArm(OOI.Target_Bone); - // scene.add(helper); + scene.add(helper); }, [cloned, gltf, setIkSolver]); diff --git a/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts b/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts index e450bfd..ab89507 100644 --- a/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts +++ b/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts @@ -1,10 +1,19 @@ import { useRef, useState } from "react"; import * as THREE from "three"; import { ThreeEvent, useThree } from "@react-three/fiber"; +import { useProductStore } from "../../../../store/simulation/useProductStore"; +import { + useSelectedEventData, + useSelectedProduct, +} from "../../../../store/simulation/useSimulationStore"; type OnUpdateCallback = (object: THREE.Object3D) => void; export default function useDraggableGLTF(onUpdate: OnUpdateCallback) { + const { getEventByModelUuid, updateAction, getActionByUuid } = + useProductStore(); + const { selectedEventData } = useSelectedEventData(); + const { selectedProduct } = useSelectedProduct(); const { camera, gl, controls } = useThree(); const activeObjRef = useRef(null); const planeRef = useRef( @@ -34,7 +43,6 @@ export default function useDraggableGLTF(onUpdate: OnUpdateCallback) { activeObjRef.current = obj; initialPositionRef.current.copy(obj.position); - // Get world position setObjectWorldPos(obj.getWorldPosition(objectWorldPos)); @@ -62,57 +70,84 @@ export default function useDraggableGLTF(onUpdate: OnUpdateCallback) { const handlePointerMove = (e: PointerEvent) => { if (!activeObjRef.current) return; + if (selectedEventData?.data.type === "roboticArm") { + const selectedArmBot = getEventByModelUuid( + selectedProduct.productId, + selectedEventData.data.modelUuid + ); + if (!selectedArmBot) return; + // Check if Shift key is pressed + const isShiftKeyPressed = e.shiftKey; - // Check if Shift key is pressed - const isShiftKeyPressed = e.shiftKey; + // Get the mouse position relative to the canvas + const rect = gl.domElement.getBoundingClientRect(); + pointer.x = ((e.clientX - rect.left) / rect.width) * 2 - 1; + pointer.y = -((e.clientY - rect.top) / rect.height) * 2 + 1; - // Get the mouse position relative to the canvas - const rect = gl.domElement.getBoundingClientRect(); - pointer.x = ((e.clientX - rect.left) / rect.width) * 2 - 1; - pointer.y = -((e.clientY - rect.top) / rect.height) * 2 + 1; + // Update raycaster to point to the mouse position + raycaster.setFromCamera(pointer, camera); - // Update raycaster to point to the mouse position - raycaster.setFromCamera(pointer, camera); + // Create a vector to store intersection point + const intersection = new THREE.Vector3(); + const intersects = raycaster.ray.intersectPlane( + planeRef.current, + intersection + ); + if (!intersects) return; - // Create a vector to store intersection point - const intersection = new THREE.Vector3(); - const intersects = raycaster.ray.intersectPlane( - planeRef.current, - intersection - ); - if (!intersects) return; + // Add offset for dragging + intersection.add(offsetRef.current); - // Add offset for dragging - intersection.add(offsetRef.current); + // Get the parent's world matrix if exists + const parent = activeObjRef.current.parent; + const targetPosition = new THREE.Vector3(); - // Get the parent's world matrix if exists - const parent = activeObjRef.current.parent; - const targetPosition = new THREE.Vector3(); + // OnPointerDown + initialPositionRef.current.copy(objectWorldPos); - // OnPointerDown - initialPositionRef.current.copy(objectWorldPos); + // OnPointerMove + if (isShiftKeyPressed) { + const { x: initialX, y: initialY } = initialPositionRef.current; + const { x: objectX, z: objectZ } = objectWorldPos; - // OnPointerMove - if (isShiftKeyPressed) { - const { x: initialX, y: initialY } = initialPositionRef.current; - const { x: objectX, z: objectZ } = objectWorldPos; + const deltaX = intersection.x - initialX; - const deltaX = intersection.x - initialX; + targetPosition.set(objectX, initialY + deltaX, objectZ); + } else { + // For free movement + targetPosition.copy(intersection); + } - targetPosition.set(objectX, initialY + deltaX, objectZ); - } else { - // For free movement - targetPosition.copy(intersection); + // CONSTRAIN MOVEMENT HERE: + const centerX = selectedArmBot.position[0]; + const centerZ = selectedArmBot.position[2]; + const minDistance = 1.2; // 1.4 radius + const maxDistance = 2; // 2 radius + + const deltaX = targetPosition.x - centerX; + const deltaZ = targetPosition.z - centerZ; + const distance = Math.sqrt(deltaX * deltaX + deltaZ * deltaZ); + + if (distance < minDistance || distance > maxDistance) { + const angle = Math.atan2(deltaZ, deltaX); + const clampedDistance = Math.min( + Math.max(distance, minDistance), + maxDistance + ); + + targetPosition.x = centerX + Math.cos(angle) * clampedDistance; + targetPosition.z = centerZ + Math.sin(angle) * clampedDistance; + } + targetPosition.y = Math.min(Math.max(targetPosition.y, 0.6), 1.5); + // Convert world position to local if object is nested inside a parent + if (parent) { + parent.worldToLocal(targetPosition); + } + + // Update object position + + activeObjRef.current.position.copy(targetPosition); } - - // Convert world position to local if object is nested inside a parent - if (parent) { - parent.worldToLocal(targetPosition); - } - - // Update object position - - activeObjRef.current.position.copy(targetPosition); }; const handlePointerUp = () => {