From f7eef1b53f49fd4718e41760c9e4bc6dd4cb4b45 Mon Sep 17 00:00:00 2001
From: Jake <realrobotshk@gmail.com>
Date: Wed, 5 Nov 2025 14:13:45 +0800
Subject: [PATCH] arc now shows rotation constraints, also rotation constraints
 now constrain rotation

---
 script.js | 74 +++++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 66 insertions(+), 8 deletions(-)

diff --git a/script.js b/script.js
index ee6433d..b2497b1 100644
--- a/script.js
+++ b/script.js
@@ -17,7 +17,7 @@ camera.lookAt(0, 0, 0);
 // Lights
 scene.add(new THREE.AmbientLight(0xffffff, 0.6));
 const directional = new THREE.DirectionalLight(0xffffff, 0.8);
-directional.position.set(5, 10, 7);
+directional.position.set(15, 10, 7);
 scene.add(directional);
 
 // Controls
@@ -28,7 +28,7 @@ controls.update();
 // Helpers
 scene.add(new THREE.AxesHelper(1));
 const gridHelper = new THREE.GridHelper(2, 10);
-gridHelper.rotation.x = Math.PI / 2;
+//gridHelper.rotation.x = Math.PI / 2;
 gridHelper.position.z = 0;
 scene.add(gridHelper);
 
@@ -41,6 +41,10 @@ let lastX = null;
 let worldAxis = null;
 
 let draggedJoint = null;
+let arc = null;
+const jointAngles = {}; // key: jointName, value: current angle in radians
+
+
 
 
 function findJointAncestor(object) {
@@ -51,6 +55,31 @@ function findJointAncestor(object) {
     return null;
 }
 
+function createRotationArc(axis, lower, upper, radius = 0.1, segments = 32) {
+    const points = [];
+    console.log(axis);
+    console.log(axis.clone());
+    // ✅ Ensure axis is normalized
+    const normalizedAxis = axis.clone().normalize();
+
+    // ✅ Find a perpendicular vector
+    let perp = new THREE.Vector3(0, 1, 0);
+    if (Math.abs(normalizedAxis.dot(perp)) > 0.99) {
+        perp = new THREE.Vector3(1, 0, 0); // fallback if axis is nearly parallel to Y
+    }
+    const tangent = perp.cross(normalizedAxis).normalize();
+
+    for (let i = 0; i <= segments; i++) {
+        const angle = lower + (upper - lower) * (i / segments);
+        const point = tangent.clone().applyAxisAngle(normalizedAxis, angle).multiplyScalar(radius);
+        points.push(point);
+    }
+
+    const geometry = new THREE.BufferGeometry().setFromPoints(points);
+    const material = new THREE.LineBasicMaterial({ color: 0xffff00 });
+    return new THREE.Line(geometry, material);
+}
+
 
 
 // Load URDF
@@ -88,12 +117,27 @@ loader.load('./urdf/sample.urdf', robot => {
         const intersects = raycaster.intersectObjects(robot.children, true);
 
         if (isDragging && draggedJoint && worldAxis) {
-            const deltaX = event.clientX - lastX;
-            lastX = event.clientX;
-            const angle = deltaX * 0.005;
-            console.log(worldAxis, angle);
-            draggedJoint.rotateOnAxis(worldAxis, angle);
-        }
+  const deltaX = event.clientX - lastX;
+  lastX = event.clientX;
+  const angleDelta = deltaX * 0.005;
+
+  const jointName = draggedJoint.name;
+  const limits = robot.joints[jointName]?.limit;
+  const lower = limits?.lower ?? -Math.PI;
+  const upper = limits?.upper ?? Math.PI;
+
+  const currentAngle = jointAngles[jointName] ?? 0;
+  const proposedAngle = currentAngle + angleDelta;
+
+  // ✅ Clamp to limits
+  const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
+  const actualDelta = clampedAngle - currentAngle;
+
+  // ✅ Apply rotation
+  draggedJoint.rotateOnAxis(worldAxis, actualDelta);
+  jointAngles[jointName] = clampedAngle;
+}
+
 
 
         if (intersects.length > 0) {
@@ -142,6 +186,8 @@ loader.load('./urdf/sample.urdf', robot => {
         controls.enabled = false;
 
         draggedJoint = findJointAncestor(hoveredJoint);
+
+
         if (!draggedJoint) return;
 
         const jointName = draggedJoint.name;
@@ -160,6 +206,16 @@ loader.load('./urdf/sample.urdf', robot => {
         }
 
         worldAxis = urdfAxis.normalize();
+        const limits = draggedJoint?.limit;
+        console.log(limits);
+        const lower = limits?.lower ?? -Math.PI;
+        const upper = limits?.upper ?? Math.PI;
+        arc = createRotationArc(worldAxis, lower, upper);
+        draggedJoint.parent.add(arc);
+        arc.position.copy(draggedJoint.position);
+        jointAngles[jointName] ??= 0;
+
+
 
         console.log(`Dragging joint "${jointName}" on axis`, worldAxis.toArray());
     });
@@ -172,6 +228,8 @@ loader.load('./urdf/sample.urdf', robot => {
     canvas.addEventListener('pointerup', () => {
         isDragging = false;
         controls.enabled = true;
+        draggedJoint.parent.remove(arc);
+
         draggedJoint = null;
         worldAxis = null;
     });