even more kinematics

test_angles.js

@@ -0,0 +1,20 @@
+const d2 = 12.9, l1 = 85.0, l2 = 110.0;
+for(let t1=0; t1<360; t1++){
+  for(let t2=0; t2<360; t2++){
+    let th1 = t1 * Math.PI/180; let th2 = t2 * Math.PI/180;
+    let e1x = -d2 + l1*Math.cos(th1); let e1y = l1*Math.sin(th1);
+    let e2x = d2 + l1*Math.cos(th2); let e2y = l1*Math.sin(th2);
+    let dx = e2x - e1x; let dy = e2y - e1y;
+    let dist = Math.sqrt(dx*dx + dy*dy);
+    if(dist < 1e-6 || dist > 2*l2) continue;
+    let a = dist/2; let h = Math.sqrt(Math.max(0, l2*l2 - a*a));
+    let mx = (e1x+e2x)/2; let my = (e1y+e2y)/2;
+    let px1 = mx + h*(dy/dist); let py1 = my + h*(-dx/dist);
+    let px2 = mx - h*(dy/dist); let py2 = my - h*(-dx/dist);
+    let useFirst = py1 >= py2;
+    let ex = useFirst ? px1 : px2; let ey = useFirst ? py1 : py2;
+    if(Math.abs(ex - 114.4) < 1.0 && Math.abs(ey - 112.7) < 1.0){
+      console.log('Match: t1=' + t1 + ' t2=' + t2 + ' -> ' + ex.toFixed(1) + ', ' + ey.toFixed(1));
+    }
+  }
+}

test_angles.py

@@ -0,0 +1,48 @@
+import math
+
+d2 = 12.9
+l1 = 85.0
+l2 = 110.0
+
+for t1 in range(360):
+    for t2 in range(360):
+        th1 = math.radians(t1)
+        th2 = math.radians(t2)
+        
+        e1x = -d2 + l1 * math.cos(th1)
+        e1y = l1 * math.sin(th1)
+        
+        e2x = d2 + l1 * math.cos(th2)
+        e2y = l1 * math.sin(th2)
+        
+        dx = e2x - e1x
+        dy = e2y - e1y
+        dist = math.sqrt(dx*dx + dy*dy)
+        
+        if dist < 1e-6 or dist > 2*l2:
+            continue
+            
+        a = dist / 2
+        h2 = l2*l2 - a*a
+        if h2 < 0:
+            h2 = 0
+        h = math.sqrt(h2)
+        
+        mx = (e1x + e2x) / 2
+        my = (e1y + e2y) / 2
+        
+        px1 = mx + h * (dy / dist)
+        py1 = my + h * (-dx / dist)
+        
+        px2 = mx - h * (dy / dist)
+        py2 = my - h * (-dx / dist)
+        
+        if py1 >= py2:
+            ex = px1
+            ey = py1
+        else:
+            ex = px2
+            ey = py2
+            
+        if abs(ex - 114.4) < 1.0 and abs(ey - 112.7) < 1.0:
+            print(f"Match: t1={t1} t2={t2} -> {ex:.1f}, {ey:.1f}")

web/js/kinematics.js

@@ -245,21 +245,63 @@ function forward(theta1, theta2) {
  */
 function armsCrossed(theta1, theta2) {
   const { d2, l1 } = ARM;
-  const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
+  const { BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX } = LIMITS;
 
-  // Elbow positions (same formula as forward())
+  const m1x = -d2, m1y = 0;
+  const m2x = d2, m2y = 0;
+  
   const e1x = -d2 + l1 * Math.cos(theta1);
   const e1y = l1 * Math.sin(theta1);
   const e2x = +d2 + l1 * Math.cos(theta2);
   const e2y = l1 * Math.sin(theta2);
 
-  // Elbow1 (from M1 on the LEFT) must not appear far to the RIGHT at low height
+  // Helper: line segment (x1,y1)->(x2,y2) intersects AABB (minX,maxX,minY,maxY)
-  // Elbow2 (from M2 on the RIGHT) must not appear far to the LEFT at low height
+  function lineIntersectsBox(x1, y1, x2, y2, minX, maxX, minY, maxY) {
-  // Both conditions together catch the "arms have swapped sides" scenario.
+    if (Math.max(x1, x2) < minX || Math.min(x1, x2) > maxX) return false;
-  const e1_crossed = e1x > XI && e1y < YM;  // M1's elbow went too far right
+    if (Math.max(y1, y2) < minY || Math.min(y1, y2) > maxY) return false;
-  const e2_crossed = e2x < -XI && e2y < YM;  // M2's elbow went too far left
 
-  return e1_crossed || e2_crossed;
+    // Check if line crosses the box
+    const outcodes = (x, y) => {
+      let code = 0;
+      if (x < minX) code |= 1; else if (x > maxX) code |= 2;
+      if (y < minY) code |= 4; else if (y > maxY) code |= 8;
+      return code;
+    };
+    
+    let c1 = outcodes(x1, y1);
+    let c2 = outcodes(x2, y2);
+    if ((c1 & c2) !== 0) return false; // Both endpoints share an outside zone
+    if (c1 === 0 || c2 === 0) return true; // One endpoint is inside
+
+    // Detailed check: find intersection points with box boundaries
+    const m = (y2 - y1) / (x2 - x1);
+    const b = y1 - m * x1;
+    
+    // Check Left/Right edges
+    if (x1 !== x2) {
+      let yLeft = m * minX + b;
+      if (yLeft >= minY && yLeft <= maxY) return true;
+      let yRight = m * maxX + b;
+      if (yRight >= minY && yRight <= maxY) return true;
+    }
+    // Check Top/Bottom edges
+    if (y1 !== y2) {
+      let xBottom = (minY - b) / m;
+      if (xBottom >= minX && xBottom <= maxX) return true;
+      let xTop = (maxY - b) / m;
+      if (xTop >= minX && xTop <= maxX) return true;
+    }
+    return false;
+  }
+
+  const cross1 = lineIntersectsBox(m1x, m1y, e1x, e1y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
+  const cross2 = lineIntersectsBox(m2x, m2y, e2x, e2y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
+  
+  // Also check if elbows went rogue (e.g. wrapped entirely around the wrong side)
+  const e1_rogue = e1x > BOX_HALF_W && e1y < BOX_Y_MAX;
+  const e2_rogue = e2x < -BOX_HALF_W && e2y < BOX_Y_MAX;
+
+  return cross1 || cross2 || e1_rogue || e2_rogue;
 }
 
 // ── Workspace check ───────────────────────────────────────────────