diff --git a/test_angles.js b/test_angles.js new file mode 100644 index 0000000..898a09a --- /dev/null +++ b/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)); + } + } +} diff --git a/test_angles.py b/test_angles.py new file mode 100644 index 0000000..fe906d9 --- /dev/null +++ b/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}") diff --git a/web/js/kinematics.js b/web/js/kinematics.js index f016ce2..3295f82 100644 --- a/web/js/kinematics.js +++ b/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 - // Elbow2 (from M2 on the RIGHT) must not appear far to the LEFT at low height - // Both conditions together catch the "arms have swapped sides" scenario. - const e1_crossed = e1x > XI && e1y < YM; // M1's elbow went too far right - const e2_crossed = e2x < -XI && e2y < YM; // M2's elbow went too far left + // Helper: line segment (x1,y1)->(x2,y2) intersects AABB (minX,maxX,minY,maxY) + function lineIntersectsBox(x1, y1, x2, y2, minX, maxX, minY, maxY) { + if (Math.max(x1, x2) < minX || Math.min(x1, x2) > maxX) return false; + if (Math.max(y1, y2) < minY || Math.min(y1, y2) > maxY) return false; - 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 ───────────────────────────────────────────────