diff --git a/web/js/kinematics.js b/web/js/kinematics.js index 8f17b8e..c254a5d 100644 --- a/web/js/kinematics.js +++ b/web/js/kinematics.js @@ -245,64 +245,20 @@ function forward(theta1, theta2) { */ function armsCrossed(theta1, theta2) { const { d2, l1 } = ARM; - const { BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX } = LIMITS; + const { ELBOW_BOX_X_INNER, ELBOW_BOX_Y_MAX } = LIMITS; - 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); - // 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; - - // 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; - } - - // Motors are at Y=0, so they are inside BOX_Y_MIN=-10. Start the collision box above the motors. - const cross1 = lineIntersectsBox(m1x, m1y, e1x, e1y, -BOX_HALF_W, BOX_HALF_W, 15, BOX_Y_MAX); - const cross2 = lineIntersectsBox(m2x, m2y, e2x, e2y, -BOX_HALF_W, BOX_HALF_W, 15, BOX_Y_MAX); + // E1 (left motor) should not cross to the right side (X > -ELBOW_BOX_X_INNER) if it's low (Y < ELBOW_BOX_Y_MAX) + if (e1y < ELBOW_BOX_Y_MAX && e1x > -ELBOW_BOX_X_INNER) return true; - // 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; + // E2 (right motor) should not cross to the left side (X < ELBOW_BOX_X_INNER) if it's low (Y < ELBOW_BOX_Y_MAX) + if (e2y < ELBOW_BOX_Y_MAX && e2x < ELBOW_BOX_X_INNER) return true; - return cross1 || cross2 || e1_rogue || e2_rogue; + return false; } // ── Workspace check ───────────────────────────────────────────────