osiu97/ESP32-WijiBoard
1
0
Fork 0
Code Issues 10 Pull Requests Packages Projects Releases Wiki Activity

new bounding box - kinematics

Browse Source
This commit is contained in:
PROFERIS - Mi³osz Stocki
2026-07-08 14:59:19 +02:00
parent 242df22ba6
commit eca780df95
1 changed files with 6 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/js/kinematics.js
+6 -50
View File
@@ -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 ───────────────────────────────────────────────