RRT: Random-Tree Path Planning

// RRT: Rapidly-exploring Random Tree path planning.
// Tree grows from start, biased 10% toward goal. Step length is fixed.
// Once a node lands within EPS of goal, traces path back to root.

const STEP = 14;          // extension length
const EPS = 18;           // goal radius
const GOAL_BIAS = 0.10;
const EXTS_PER_FRAME = 6; // extensions per tick
const MAX_NODES = 6000;
const HOLD_FRAMES = 90;   // pause after solved before restart

let W = 0, H = 0;
let nodes;          // {x, y, parent}
let obstacles;      // {x, y, r}
let userObstacles;  // persisted across regens
let start, goal;
let solved;
let solvedPath;
let solvedFrames;
let failedAttempts;
let nodeCount;

function rng(min, max) { return min + Math.random() * (max - min); }

function dist2(ax, ay, bx, by) {
  const dx = ax - bx, dy = ay - by;
  return dx * dx + dy * dy;
}

function pointInObstacle(x, y) {
  for (let i = 0; i < obstacles.length; i++) {
    const o = obstacles[i];
    const dx = x - o.x, dy = y - o.y;
    if (dx * dx + dy * dy <= o.r * o.r) return true;
  }
  return false;
}

// Segment-circle test: does segment (x1,y1)->(x2,y2) intersect obstacle circle?
function segmentHitsObstacle(x1, y1, x2, y2) {
  const dx = x2 - x1, dy = y2 - y1;
  const len2 = dx * dx + dy * dy;
  for (let i = 0; i < obstacles.length; i++) {
    const o = obstacles[i];
    // project obstacle center onto segment
    let t = ((o.x - x1) * dx + (o.y - y1) * dy) / (len2 || 1);
    if (t < 0) t = 0; else if (t > 1) t = 1;
    const px = x1 + t * dx, py = y1 + t * dy;
    const ex = px - o.x, ey = py - o.y;
    if (ex * ex + ey * ey <= o.r * o.r) return true;
  }
  return false;
}

function makeObstacles() {
  const arr = [];
  const count = 7 + (Math.random() * 4) | 0; // 7-10
  const margin = 50;
  let tries = 0;
  while (arr.length < count && tries < 400) {
    tries++;
    const r = rng(28, 58);
    const x = rng(r + 10, W - r - 10);
    const y = rng(r + 10, H - r - 10);
    // keep clear of start and goal
    if (dist2(x, y, start.x, start.y) < (r + margin) * (r + margin)) continue;
    if (dist2(x, y, goal.x, goal.y) < (r + margin) * (r + margin)) continue;
    arr.push({ x, y, r });
  }
  // append user-placed obstacles, clamped/skipped if they cover start/goal
  for (const u of userObstacles) {
    if (dist2(u.x, u.y, start.x, start.y) < (u.r + 8) * (u.r + 8)) continue;
    if (dist2(u.x, u.y, goal.x, goal.y) < (u.r + 8) * (u.r + 8)) continue;
    arr.push(u);
  }
  return arr;
}

function resetRun(regenObstacles) {
  if (!start) start = { x: 60, y: H - 60 };
  if (!goal) goal = { x: W - 60, y: 60 };
  start.x = 60; start.y = H - 60;
  goal.x = W - 60; goal.y = 60;
  if (regenObstacles) obstacles = makeObstacles();
  nodes = new Float32Array(MAX_NODES * 3); // x, y, parentIdx (or -1)
  nodes[0] = start.x; nodes[1] = start.y; nodes[2] = -1;
  nodeCount = 1;
  solved = false;
  solvedPath = null;
  solvedFrames = 0;
  failedAttempts = 0;
}

function init({ ctx, width, height }) {
  W = width; H = height;
  start = { x: 60, y: H - 60 };
  goal = { x: W - 60, y: 60 };
  userObstacles = [];
  resetRun(true);
  ctx.fillStyle = '#06070b';
  ctx.fillRect(0, 0, W, H);
}

function nearestIdx(x, y) {
  let bestI = 0;
  let bestD = Infinity;
  for (let i = 0; i < nodeCount; i++) {
    const dx = x - nodes[i * 3];
    const dy = y - nodes[i * 3 + 1];
    const d = dx * dx + dy * dy;
    if (d < bestD) { bestD = d; bestI = i; }
  }
  return bestI;
}

function extendOnce() {
  if (nodeCount >= MAX_NODES) return false;
  // sample
  let sx, sy;
  if (Math.random() < GOAL_BIAS) {
    sx = goal.x; sy = goal.y;
  } else {
    sx = Math.random() * W;
    sy = Math.random() * H;
  }
  const ni = nearestIdx(sx, sy);
  const nx0 = nodes[ni * 3], ny0 = nodes[ni * 3 + 1];
  const dx = sx - nx0, dy = sy - ny0;
  const d = Math.hypot(dx, dy);
  if (d < 1e-3) return false;
  const ux = dx / d, uy = dy / d;
  const newX = nx0 + ux * STEP;
  const newY = ny0 + uy * STEP;
  if (newX < 2 || newX > W - 2 || newY < 2 || newY > H - 2) return false;
  if (pointInObstacle(newX, newY)) return false;
  if (segmentHitsObstacle(nx0, ny0, newX, newY)) return false;
  const idx = nodeCount;
  nodes[idx * 3] = newX;
  nodes[idx * 3 + 1] = newY;
  nodes[idx * 3 + 2] = ni;
  nodeCount++;
  // goal reached?
  const gdx = newX - goal.x, gdy = newY - goal.y;
  if (gdx * gdx + gdy * gdy <= EPS * EPS) {
    if (!segmentHitsObstacle(newX, newY, goal.x, goal.y)) {
      // build path: goal <- new node <- ... <- root
      const path = [];
      path.push({ x: goal.x, y: goal.y });
      let cur = idx;
      while (cur !== -1) {
        path.push({ x: nodes[cur * 3], y: nodes[cur * 3 + 1] });
        cur = nodes[cur * 3 + 2] | 0;
      }
      path.reverse(); // start ... goal? we pushed goal first then ascended; reverse to start->goal
      // After reverse: [start, ..., new, goal]
      solvedPath = path;
      solved = true;
      solvedFrames = 0;
    }
  }
  return true;
}

function tick({ ctx, width, height, input }) {
  if (width !== W || height !== H) {
    W = width; H = height;
    resetRun(true);
  }

  // click to add an obstacle
  for (const c of input.consumeClicks()) {
    const r = 22 + Math.random() * 14;
    // don't smother start/goal
    if (dist2(c.x, c.y, start.x, start.y) < (r + 12) * (r + 12)) continue;
    if (dist2(c.x, c.y, goal.x, goal.y) < (r + 12) * (r + 12)) continue;
    userObstacles.push({ x: c.x, y: c.y, r });
    // force a re-plan against the new obstacle set
    resetRun(true);
  }

  // step the planner
  if (!solved) {
    for (let i = 0; i < EXTS_PER_FRAME; i++) {
      extendOnce();
      if (solved) break;
      if (nodeCount >= MAX_NODES) {
        failedAttempts++;
        resetRun(true);
        break;
      }
    }
  } else {
    solvedFrames++;
    if (solvedFrames > HOLD_FRAMES) {
      resetRun(true);
    }
  }

  // --- draw ---
  ctx.fillStyle = '#06070b';
  ctx.fillRect(0, 0, W, H);

  // obstacles
  for (let i = 0; i < obstacles.length; i++) {
    const o = obstacles[i];
    ctx.fillStyle = '#1a1d28';
    ctx.beginPath();
    ctx.arc(o.x, o.y, o.r, 0, Math.PI * 2);
    ctx.fill();
    ctx.strokeStyle = '#2c3245';
    ctx.lineWidth = 1;
    ctx.stroke();
  }

  // tree edges
  ctx.strokeStyle = 'rgba(110, 170, 255, 0.45)';
  ctx.lineWidth = 1;
  ctx.beginPath();
  for (let i = 1; i < nodeCount; i++) {
    const p = nodes[i * 3 + 2] | 0;
    if (p < 0) continue;
    ctx.moveTo(nodes[p * 3], nodes[p * 3 + 1]);
    ctx.lineTo(nodes[i * 3], nodes[i * 3 + 1]);
  }
  ctx.stroke();

  // tree nodes (small dots)
  ctx.fillStyle = 'rgba(180, 210, 255, 0.65)';
  for (let i = 1; i < nodeCount; i++) {
    ctx.fillRect(nodes[i * 3] - 1, nodes[i * 3 + 1] - 1, 2, 2);
  }

  // solved path
  if (solved && solvedPath) {
    ctx.strokeStyle = '#4ade80';
    ctx.lineWidth = 3;
    ctx.lineJoin = 'round';
    ctx.lineCap = 'round';
    ctx.beginPath();
    ctx.moveTo(solvedPath[0].x, solvedPath[0].y);
    for (let i = 1; i < solvedPath.length; i++) {
      ctx.lineTo(solvedPath[i].x, solvedPath[i].y);
    }
    ctx.stroke();
    // node dots along path
    ctx.fillStyle = '#86efac';
    for (let i = 0; i < solvedPath.length; i++) {
      ctx.beginPath();
      ctx.arc(solvedPath[i].x, solvedPath[i].y, 2.5, 0, Math.PI * 2);
      ctx.fill();
    }
  }

  // goal marker (target rings)
  ctx.strokeStyle = '#f87171';
  ctx.lineWidth = 2;
  ctx.beginPath();
  ctx.arc(goal.x, goal.y, EPS, 0, Math.PI * 2);
  ctx.stroke();
  ctx.fillStyle = '#f87171';
  ctx.beginPath();
  ctx.arc(goal.x, goal.y, 4, 0, Math.PI * 2);
  ctx.fill();

  // start marker
  ctx.fillStyle = '#22d3ee';
  ctx.beginPath();
  ctx.arc(start.x, start.y, 6, 0, Math.PI * 2);
  ctx.fill();
  ctx.strokeStyle = 'rgba(34, 211, 238, 0.45)';
  ctx.lineWidth = 1.5;
  ctx.beginPath();
  ctx.arc(start.x, start.y, 10, 0, Math.PI * 2);
  ctx.stroke();

  // HUD
  ctx.fillStyle = 'rgba(220, 230, 245, 0.85)';
  ctx.font = '12px monospace';
  ctx.fillText('nodes: ' + nodeCount, 10, 18);
  ctx.fillText(solved ? 'path found' : 'planning…', 10, 34);
  ctx.fillStyle = 'rgba(34, 211, 238, 0.9)';
  ctx.fillText('start', start.x + 12, start.y + 4);
  ctx.fillStyle = 'rgba(248, 113, 113, 0.9)';
  ctx.fillText('goal', goal.x - 30, goal.y - 8);
}