diff --git a/app/src/modules/simulation/roboticArm/instances/animator/materialAnimator.tsx b/app/src/modules/simulation/roboticArm/instances/animator/materialAnimator.tsx index 893f759..b458d32 100644 --- a/app/src/modules/simulation/roboticArm/instances/animator/materialAnimator.tsx +++ b/app/src/modules/simulation/roboticArm/instances/animator/materialAnimator.tsx @@ -1,17 +1,16 @@ import { useFrame } from '@react-three/fiber'; import React, { useEffect, useRef, useState } from 'react'; import * as THREE from 'three'; -import { useLogger } from '../../../../../components/ui/log/LoggerContext'; +import { MaterialModel } from '../../../materials/instances/material/materialModel'; type MaterialAnimatorProps = { ikSolver: any; - armBot: any; + armBot: ArmBotStatus; currentPhase: string; }; export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: MaterialAnimatorProps) { - const sphereRef = useRef(null); - const boneWorldPosition = new THREE.Vector3(); + const sphereRef = useRef(null); const [isRendered, setIsRendered] = useState(false); useEffect(() => { @@ -41,7 +40,8 @@ export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: Mat const direction = new THREE.Vector3(); direction.subVectors(boneWorldPos, boneTargetWorldPos).normalize(); const downwardDirection = direction.clone().negate(); - const adjustedPosition = boneWorldPos.clone().addScaledVector(downwardDirection, -0.25); + + const adjustedPosition = boneWorldPos.clone().addScaledVector(downwardDirection, -0.01); //set position sphereRef.current.position.copy(adjustedPosition); @@ -56,10 +56,10 @@ export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: Mat return ( <> {isRendered && ( - - - - + )} ); diff --git a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx index 34f191b..3ca7665 100644 --- a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx +++ b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx @@ -1,39 +1,34 @@ 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'; +import { Line, Text } from '@react-three/drei'; +import { useAnimationPlaySpeed, usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '../../../../../store/usePlayButtonStore'; + +type PointWithDegree = { + position: [number, number, number]; + degree: number; +}; + +function RoboticArmAnimator({ HandleCallback, restPosition, ikSolver, targetBone, armBot, path }: any) { -function RoboticArmAnimator({ - HandleCallback, - restPosition, - ikSolver, - targetBone, - armBot, - 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(null); - let curveHeight = 1.75 const totalDistanceRef = useRef(0); const startTimeRef = useRef(null); const segmentDistancesRef = useRef([]); + const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]); + const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]); + const [circlePointsWithDegrees, setCirclePointsWithDegrees] = useState([]); + const [customCurvePoints, setCustomCurvePoints] = useState(null); + let curveHeight = 1.75 + const CIRCLE_RADIUS = 1.6 + // Zustand stores const { isPlaying } = usePlayButtonStore(); const { isPaused } = usePauseButtonStore(); const { isReset } = useResetButtonStore(); const { speed } = useAnimationPlaySpeed(); - const CIRCLE_RADIUS = 1.6 - // Update path state whenever `path` prop changes useEffect(() => { setCurrentPath(path); @@ -47,7 +42,7 @@ function RoboticArmAnimator({ //Handle Reset Animation useEffect(() => { - if (isReset) { + if (isReset || !isPlaying) { progressRef.current = 0; curveRef.current = null; setCurrentPath([]); @@ -76,6 +71,29 @@ function RoboticArmAnimator({ } return points; } + + //Generate CirclePoints with Angle + function generateRingPointsWithDegrees(radius: number, segments: number, initialRotation: [number, number, number]) { + const points: { position: [number, number, number]; degree: number }[] = []; + for (let i = 0; i < segments; i++) { + const angleRadians = (i / segments) * Math.PI * 2; + const x = Math.cos(angleRadians) * radius; + const z = Math.sin(angleRadians) * radius; + const degree = (angleRadians * 180) / Math.PI; // Convert radians to degrees + points.push({ + position: [x, 1.5, z], + degree, + }); + } + return points; + } + + // Handle circle points based on armBot position + useEffect(() => { + const points = generateRingPointsWithDegrees(CIRCLE_RADIUS, 64, armBot.rotation); + setCirclePointsWithDegrees(points) + }, [armBot.rotation]); + // Function for find nearest Circlepoints Index const findNearestIndex = (nearestPoint: [number, number, number], points: [number, number, number][], epsilon = 1e-6) => { for (let i = 0; i < points.length; i++) { @@ -100,6 +118,30 @@ function RoboticArmAnimator({ }, circlePoints[0]); }; + // Helper function to collect points and check forbidden degrees + const collectArcPoints = (startIdx: number, endIdx: number, clockwise: boolean) => { + const totalSegments = 64; + const arcPoints: [number, number, number][] = []; + let i = startIdx; + + while (i !== (endIdx + (clockwise ? 1 : -1) + totalSegments) % totalSegments) { + const { degree, position } = circlePointsWithDegrees[i]; + // Skip over + arcPoints.push(position); + i = (i + (clockwise ? 1 : -1) + totalSegments) % totalSegments; + } + return arcPoints; + }; + + //Range to restrict angle + const hasForbiddenDegrees = (arc: [number, number, number][]) => { + return arc.some(p => { + const idx = findNearestIndex(p, circlePoints); + const degree = circlePointsWithDegrees[idx]?.degree || 0; + return degree >= 271 && degree <= 300; // Forbidden range: 271° to 300° + }); + }; + // Handle nearest points and final path (including arc points) useEffect(() => { if (circlePoints.length > 0 && currentPath.length > 0) { @@ -116,31 +158,23 @@ function RoboticArmAnimator({ const indexOfNearestStart = findNearestIndex(nearestToStart, circlePoints); const indexOfNearestEnd = findNearestIndex(nearestToEnd, circlePoints); - const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + 64) % 64; - const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + 64) % 64; - const clockwiseIsShorter = clockwiseDistance <= counterClockwiseDistance; + const totalSegments = 64; + const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + totalSegments) % totalSegments; + const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + totalSegments) % totalSegments; + + // Try both directions + const arcClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, true); + const arcCounterClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, false); + + const clockwiseForbidden = hasForbiddenDegrees(arcClockwise); + const counterClockwiseForbidden = hasForbiddenDegrees(arcCounterClockwise); let arcPoints: [number, number, number][] = []; - if (clockwiseIsShorter) { - if (indexOfNearestStart <= indexOfNearestEnd) { - arcPoints = circlePoints.slice(indexOfNearestStart, indexOfNearestEnd + 1); - } else { - arcPoints = [ - ...circlePoints.slice(indexOfNearestStart, 64), - ...circlePoints.slice(0, indexOfNearestEnd + 1) - ]; - } + if (!clockwiseForbidden && (clockwiseDistance <= counterClockwiseDistance || counterClockwiseForbidden)) { + arcPoints = arcClockwise; } 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]); - } - } + arcPoints = arcCounterClockwise; } const pathVectors = [ @@ -153,9 +187,7 @@ function RoboticArmAnimator({ new THREE.Vector3(end[0], end[1], end[2]) ]; - const pathSegments: [THREE.Vector3, THREE.Vector3][] = []; - for (let i = 0; i < pathVectors.length - 1; i++) { pathSegments.push([pathVectors[i], pathVectors[i + 1]]); } @@ -165,13 +197,7 @@ function RoboticArmAnimator({ 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]); @@ -182,7 +208,6 @@ function RoboticArmAnimator({ const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBone); if (!bone) return; - if (isPlaying) { if (isReset) { bone.position.copy(restPosition); @@ -227,12 +252,17 @@ function RoboticArmAnimator({ ikSolver.update(); } } else if (!isPlaying && currentPath.length === 0) { + progressRef.current = 0; + startTimeRef.current = null; + setCurrentPath([]); + setCustomCurvePoints([]); bone.position.copy(restPosition); + } ikSolver.update(); }); - + //Helper to Visible the Circle and Curve return ( <> {customCurvePoints && customCurvePoints?.length >= 2 && currentPath && isPlaying && ( @@ -245,10 +275,18 @@ function RoboticArmAnimator({ /> )} - - - - + + {/* Green ring */} + + + + + + + ); } diff --git a/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx b/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx index 8cb0d13..035f25e 100644 --- a/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx +++ b/app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx @@ -5,7 +5,6 @@ import { usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '.. import { useArmBotStore } from '../../../../../store/simulation/useArmBotStore'; import armModel from "../../../../../assets/gltf-glb/rigged/ik_arm_1.glb"; import { useThree } from "@react-three/fiber"; -import useModuleStore from '../../../../../store/useModuleStore'; import * as THREE from "three"; import MaterialAnimator from '../animator/materialAnimator'; @@ -24,16 +23,15 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { let startTime: number; //zustand const { addCurrentAction, setArmBotActive, setArmBotState, removeCurrentAction } = useArmBotStore(); - const { activeModule } = useModuleStore(); const { isPlaying } = usePlayButtonStore(); - const { isReset, setReset } = useResetButtonStore(); + const { isReset } = useResetButtonStore(); const { isPaused } = usePauseButtonStore(); - function firstFrame() { startTime = performance.now(); step(); } + function step() { if (isPausedRef.current) { if (!pauseTimeRef.current) { @@ -87,6 +85,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.") } } + useEffect(() => { isPausedRef.current = isPaused; }, [isPaused]); @@ -98,6 +97,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { setArmBotState(armBot.modelUuid, "idle") setCurrentPhase("init"); setPath([]) + setIkSolver(null); removeCurrentAction(armBot.modelUuid) isPausedRef.current = false pauseTimeRef.current = null @@ -117,17 +117,17 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { useEffect(() => { const targetMesh = scene?.getObjectByProperty("uuid", armBot.modelUuid); if (targetMesh) { - targetMesh.visible = activeModule !== "simulation" + targetMesh.visible = (!isPlaying) } const targetBones = ikSolver?.mesh.skeleton.bones.find((b: any) => b.name === targetBone); - if (isPlaying) { + if (!isReset && isPlaying) { //Moving armBot from initial point to rest position. if (!armBot?.isActive && armBot?.state == "idle" && currentPhase == "init") { - setArmBotActive(armBot.modelUuid, true) - setArmBotState(armBot.modelUuid, "running") - setCurrentPhase("init-to-rest"); if (targetBones) { - let curve = createCurveBetweenTwoPoints(targetBones.position, targetBones.position) + setArmBotActive(armBot.modelUuid, true) + setArmBotState(armBot.modelUuid, "running") + setCurrentPhase("init-to-rest"); + let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition) if (curve) { setPath(curve.points.map(point => [point.x, point.y, point.z])); } @@ -142,9 +142,9 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { }, 3000); return () => clearTimeout(timeoutId); } + //Moving to pickup point else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && armBot.currentAction) { if (armBot.currentAction) { - setArmBotActive(armBot.modelUuid, true); setArmBotState(armBot.modelUuid, "running"); setCurrentPhase("rest-to-start"); @@ -158,15 +158,29 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { } logStatus(armBot.modelUuid, "Moving armBot from rest point to start position.") } + // Moving to Pick to Drop position else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "picking" && armBot.currentAction) { requestAnimationFrame(firstFrame); } + //Moving to drop point to restPosition else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "dropping" && armBot.currentAction) { requestAnimationFrame(firstFrame); } + } else { + logStatus(armBot.modelUuid, "Simulation Play Exited") + setArmBotActive(armBot.modelUuid, false) + setArmBotState(armBot.modelUuid, "idle") + setCurrentPhase("init"); + setIkSolver(null); + setPath([]) + isPausedRef.current = false + pauseTimeRef.current = null + isPausedRef.current = false + startTime = 0 + removeCurrentAction(armBot.modelUuid) } - }, [currentPhase, armBot, isPlaying, ikSolver]) + }, [currentPhase, armBot, isPlaying, isReset, ikSolver]) function createCurveBetweenTwoPoints(p1: any, p2: any) { @@ -213,9 +227,13 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) { return ( <> - - + {!isReset && isPlaying && ( + <> + + + + )} ) diff --git a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx index be41a95..4bd05a6 100644 --- a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx +++ b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx @@ -11,7 +11,7 @@ type IKInstanceProps = { modelUrl: string; ikSolver: any; setIkSolver: any - armBot: any; + armBot: ArmBotStatus; groupRef: any; }; function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKInstanceProps) { @@ -57,12 +57,6 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns rotationMin: new THREE.Vector3(0, 0, 0), rotationMax: new THREE.Vector3(2, 0, 0), }, - // { - // index: 1, - // enabled: true, - // rotationMin: new THREE.Vector3(0, -Math.PI, 0), - // rotationMax: new THREE.Vector3(0, Math.PI, 0), - // }, { index: 1, enabled: true, limitation: new THREE.Vector3(0, 1, 0) }, { index: 0, enabled: false, limitation: new THREE.Vector3(0, 0, 0) }, ], @@ -76,7 +70,7 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns setSelectedArm(OOI.Target_Bone); - scene.add(helper); + // scene.add(helper); }, [cloned, gltf, setIkSolver]); @@ -86,10 +80,10 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns setSelectedArm(groupRef.current?.getObjectByName(targetBoneName)) }}> {/* {selectedArm && } */} diff --git a/app/src/modules/simulation/roboticArm/roboticArm.tsx b/app/src/modules/simulation/roboticArm/roboticArm.tsx index 5440abf..91ccb55 100644 --- a/app/src/modules/simulation/roboticArm/roboticArm.tsx +++ b/app/src/modules/simulation/roboticArm/roboticArm.tsx @@ -39,4 +39,4 @@ function RoboticArm() { ); } -export default RoboticArm; +export default RoboticArm; \ No newline at end of file diff --git a/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts b/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts index dae90ee..e1f7fbf 100644 --- a/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts +++ b/app/src/modules/simulation/ui/arm/useDraggableGLTF.ts @@ -3,166 +3,200 @@ import * as THREE from "three"; import { ThreeEvent, useThree } from "@react-three/fiber"; import { useProductStore } from "../../../../store/simulation/useProductStore"; import { - useSelectedEventData, - useSelectedProduct, + 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( - new THREE.Plane(new THREE.Vector3(0, 1, 0), 0) - ); - const offsetRef = useRef(new THREE.Vector3()); - const initialPositionRef = useRef(new THREE.Vector3()); + const { getEventByModelUuid, updateAction, getActionByUuid } = + useProductStore(); + const { selectedEventData } = useSelectedEventData(); + const { selectedProduct } = useSelectedProduct(); + const { camera, gl, controls } = useThree(); + const activeObjRef = useRef(null); + const planeRef = useRef( + new THREE.Plane(new THREE.Vector3(0, 1, 0), 0) + ); + const offsetRef = useRef(new THREE.Vector3()); + const initialPositionRef = useRef(new THREE.Vector3()); - const raycaster = new THREE.Raycaster(); - const pointer = new THREE.Vector2(); - const [objectWorldPos, setObjectWorldPos] = useState(new THREE.Vector3()); + const raycaster = new THREE.Raycaster(); + const pointer = new THREE.Vector2(); + const [objectWorldPos, setObjectWorldPos] = useState(new THREE.Vector3()); - const handlePointerDown = (e: ThreeEvent) => { - e.stopPropagation(); + const handlePointerDown = (e: ThreeEvent) => { + e.stopPropagation(); - let obj: THREE.Object3D | null = e.object; + let obj: THREE.Object3D | null = e.object; - // Traverse up until we find modelUuid in userData - while (obj && !obj.userData?.modelUuid) { - obj = obj.parent; - } + // Traverse up until we find modelUuid in userData + while (obj && !obj.userData?.modelUuid) { + obj = obj.parent; + } - if (!obj) return; + if (!obj) return; - // Disable orbit controls while dragging - if (controls) (controls as any).enabled = false; + // Disable orbit controls while dragging + if (controls) (controls as any).enabled = false; - activeObjRef.current = obj; - initialPositionRef.current.copy(obj.position); + activeObjRef.current = obj; + initialPositionRef.current.copy(obj.position); - // Get world position - setObjectWorldPos(obj.getWorldPosition(objectWorldPos)); + // Get world position + setObjectWorldPos(obj.getWorldPosition(objectWorldPos)); - // Set plane at the object's Y level - planeRef.current.set(new THREE.Vector3(0, 1, 0), -objectWorldPos.y); + // Set plane at the object's Y level + planeRef.current.set(new THREE.Vector3(0, 1, 0), -objectWorldPos.y); - // Convert pointer to NDC - 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; + // Convert pointer to NDC + 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; - // Raycast to intersection - raycaster.setFromCamera(pointer, camera); - const intersection = new THREE.Vector3(); - raycaster.ray.intersectPlane(planeRef.current, intersection); + // Raycast to intersection + raycaster.setFromCamera(pointer, camera); + const intersection = new THREE.Vector3(); + raycaster.ray.intersectPlane(planeRef.current, intersection); - // Calculate offset - offsetRef.current.copy(objectWorldPos).sub(intersection); + // Calculate offset + offsetRef.current.copy(objectWorldPos).sub(intersection); - // Start listening for drag - gl.domElement.addEventListener("pointermove", handlePointerMove); - gl.domElement.addEventListener("pointerup", handlePointerUp); - }; + // Start listening for drag + gl.domElement.addEventListener("pointermove", handlePointerMove); + gl.domElement.addEventListener("pointerup", handlePointerUp); + }; - 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; + 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; - // 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 + // CONSTRAIN MOVEMENT HERE: + const centerX = selectedArmBot.position[0]; + const centerZ = selectedArmBot.position[2]; + const minDistance = 1.2; + const maxDistance = 2; - const deltaX = targetPosition.x - centerX; - const deltaZ = targetPosition.z - centerZ; - const distance = Math.sqrt(deltaX * deltaX + deltaZ * deltaZ); + const delta = new THREE.Vector3(targetPosition.x - centerX, 0, targetPosition.z - centerZ); - if (distance < minDistance || distance > maxDistance) { - const angle = Math.atan2(deltaZ, deltaX); - const clampedDistance = Math.min( - Math.max(distance, minDistance), - maxDistance - ); + // Create quaternion from rotation + const robotEuler = new THREE.Euler(selectedArmBot.rotation[0], selectedArmBot.rotation[1], selectedArmBot.rotation[2]); + const robotQuaternion = new THREE.Quaternion().setFromEuler(robotEuler); - 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.9); - // Convert world position to local if object is nested inside a parent - if (parent) { - parent.worldToLocal(targetPosition); - } + // Inverse rotate + const inverseQuaternion = robotQuaternion.clone().invert(); + delta.applyQuaternion(inverseQuaternion); - // Update object position + // Angle in robot local space + let relativeAngle = Math.atan2(delta.z, delta.x); + let angleDeg = (relativeAngle * 180) / Math.PI; + if (angleDeg < 0) { + angleDeg += 360; + } - activeObjRef.current.position.copy(targetPosition); - } - }; + // Clamp angle + if (angleDeg < 0 || angleDeg > 270) { + const distanceTo90 = Math.abs(angleDeg - 0); + const distanceTo270 = Math.abs(angleDeg - 270); + if (distanceTo90 < distanceTo270) { + angleDeg = 0; + } else { + return; + } + relativeAngle = (angleDeg * Math.PI) / 180; + } - const handlePointerUp = () => { - if (controls) (controls as any).enabled = true; + // Distance clamp + const distance = delta.length(); + const clampedDistance = Math.min(Math.max(distance, minDistance), maxDistance); - if (activeObjRef.current) { - // Pass the updated position to the onUpdate callback to persist it - onUpdate(activeObjRef.current); - } + // Calculate local target + const finalLocal = new THREE.Vector3( + Math.cos(relativeAngle) * clampedDistance, + 0, + Math.sin(relativeAngle) * clampedDistance + ); - gl.domElement.removeEventListener("pointermove", handlePointerMove); - gl.domElement.removeEventListener("pointerup", handlePointerUp); + // Rotate back to world space + finalLocal.applyQuaternion(robotQuaternion); - activeObjRef.current = null; - }; + targetPosition.x = centerX + finalLocal.x; + targetPosition.z = centerZ + finalLocal.z; - return { handlePointerDown }; + + // Clamp Y axis if needed + targetPosition.y = Math.min(Math.max(targetPosition.y, 0.6), 1.9); + + // Convert to local if parent exists + if (parent) { + parent.worldToLocal(targetPosition); + } + + // Update the object position + activeObjRef.current.position.copy(targetPosition); + } + }; + + const handlePointerUp = () => { + if (controls) (controls as any).enabled = true; + + if (activeObjRef.current) { + // Pass the updated position to the onUpdate callback to persist it + onUpdate(activeObjRef.current); + } + + gl.domElement.removeEventListener("pointermove", handlePointerMove); + gl.domElement.removeEventListener("pointerup", handlePointerUp); + + activeObjRef.current = null; + }; + + return { handlePointerDown }; }