v2 #80
|
|
@ -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<THREE.Mesh>(null);
|
||||
const boneWorldPosition = new THREE.Vector3();
|
||||
const sphereRef = useRef<any>(null);
|
||||
const [isRendered, setIsRendered] = useState<boolean>(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 && (
|
||||
<mesh ref={sphereRef}>
|
||||
<boxGeometry args={[0.5, 0.5, 0.5]} />
|
||||
<meshStandardMaterial color="yellow" />
|
||||
</mesh>
|
||||
<MaterialModel
|
||||
matRef={sphereRef}
|
||||
materialType={armBot.currentAction?.materialType || 'Default material'}
|
||||
/>
|
||||
)}
|
||||
</>
|
||||
);
|
||||
|
|
|
|||
|
|
@ -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<THREE.Vector3[] | null>(null);
|
||||
const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]);
|
||||
const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]);
|
||||
const [customCurvePoints, setCustomCurvePoints] = useState<THREE.Vector3[] | null>(null);
|
||||
let curveHeight = 1.75
|
||||
const totalDistanceRef = useRef(0);
|
||||
const startTimeRef = useRef<number | null>(null);
|
||||
const segmentDistancesRef = useRef<number[]>([]);
|
||||
const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]);
|
||||
const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]);
|
||||
const [circlePointsWithDegrees, setCirclePointsWithDegrees] = useState<PointWithDegree[]>([]);
|
||||
const [customCurvePoints, setCustomCurvePoints] = useState<THREE.Vector3[] | null>(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);
|
||||
if (!clockwiseForbidden && (clockwiseDistance <= counterClockwiseDistance || counterClockwiseForbidden)) {
|
||||
arcPoints = arcClockwise;
|
||||
} else {
|
||||
arcPoints = [
|
||||
...circlePoints.slice(indexOfNearestStart, 64),
|
||||
...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 {
|
||||
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({
|
|||
/>
|
||||
</mesh>
|
||||
)}
|
||||
<mesh position={[armBot.position[0], armBot.position[1] + 1.5, armBot.position[2]]} rotation={[-Math.PI / 2, 0, 0]}>
|
||||
<group
|
||||
position={[armBot.position[0], armBot.position[1] + 1.5, armBot.position[2]]}
|
||||
rotation={[armBot.rotation[0], armBot.rotation[1], armBot.rotation[2]]}
|
||||
>
|
||||
{/* Green ring */}
|
||||
<mesh rotation={[-Math.PI / 2, 0, 0]}>
|
||||
<ringGeometry args={[CIRCLE_RADIUS, CIRCLE_RADIUS + 0.02, 64]} />
|
||||
<meshBasicMaterial color="green" side={THREE.DoubleSide} />
|
||||
</mesh>
|
||||
|
||||
</group>
|
||||
|
||||
</>
|
||||
);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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") {
|
||||
if (targetBones) {
|
||||
setArmBotActive(armBot.modelUuid, true)
|
||||
setArmBotState(armBot.modelUuid, "running")
|
||||
setCurrentPhase("init-to-rest");
|
||||
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]));
|
||||
}
|
||||
|
|
@ -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) {
|
||||
|
|
@ -212,10 +226,14 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
|
|||
}
|
||||
|
||||
return (
|
||||
<>
|
||||
{!isReset && isPlaying && (
|
||||
<>
|
||||
<IKInstance modelUrl={armModel} setIkSolver={setIkSolver} ikSolver={ikSolver} armBot={armBot} groupRef={groupRef} />
|
||||
<RoboticArmAnimator HandleCallback={HandleCallback} restPosition={restPosition} ikSolver={ikSolver} targetBone={targetBone} armBot={armBot}
|
||||
logStatus={logStatus} path={path} currentPhase={currentPhase} />
|
||||
</>
|
||||
)}
|
||||
<MaterialAnimator ikSolver={ikSolver} armBot={armBot} currentPhase={currentPhase} />
|
||||
</>
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
}}>
|
||||
<primitive
|
||||
uuid={"ArmBot-X200"}
|
||||
uuid={armBot.modelUuid}
|
||||
object={cloned}
|
||||
scale={[1, 1, 1]}
|
||||
name={`arm-bot11`}
|
||||
name={armBot.modelName}
|
||||
/>
|
||||
</group>
|
||||
{/* {selectedArm && <TransformControls object={selectedArm} />} */}
|
||||
|
|
|
|||
|
|
@ -121,31 +121,65 @@ export default function useDraggableGLTF(onUpdate: OnUpdateCallback) {
|
|||
// 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 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);
|
||||
|
||||
// Inverse rotate
|
||||
const inverseQuaternion = robotQuaternion.clone().invert();
|
||||
delta.applyQuaternion(inverseQuaternion);
|
||||
|
||||
// Angle in robot local space
|
||||
let relativeAngle = Math.atan2(delta.z, delta.x);
|
||||
let angleDeg = (relativeAngle * 180) / Math.PI;
|
||||
if (angleDeg < 0) {
|
||||
angleDeg += 360;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Distance clamp
|
||||
const distance = delta.length();
|
||||
const clampedDistance = Math.min(Math.max(distance, minDistance), maxDistance);
|
||||
|
||||
// Calculate local target
|
||||
const finalLocal = new THREE.Vector3(
|
||||
Math.cos(relativeAngle) * clampedDistance,
|
||||
0,
|
||||
Math.sin(relativeAngle) * clampedDistance
|
||||
);
|
||||
|
||||
targetPosition.x = centerX + Math.cos(angle) * clampedDistance;
|
||||
targetPosition.z = centerZ + Math.sin(angle) * clampedDistance;
|
||||
}
|
||||
// Rotate back to world space
|
||||
finalLocal.applyQuaternion(robotQuaternion);
|
||||
|
||||
targetPosition.x = centerX + finalLocal.x;
|
||||
targetPosition.z = centerZ + finalLocal.z;
|
||||
|
||||
|
||||
// Clamp Y axis if needed
|
||||
targetPosition.y = Math.min(Math.max(targetPosition.y, 0.6), 1.9);
|
||||
// Convert world position to local if object is nested inside a parent
|
||||
|
||||
// Convert to local if parent exists
|
||||
if (parent) {
|
||||
parent.worldToLocal(targetPosition);
|
||||
}
|
||||
|
||||
// Update object position
|
||||
|
||||
// Update the object position
|
||||
activeObjRef.current.position.copy(targetPosition);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue