let landscape;

let start, end;

let curvePoints;

let optimal;

const N = 25;

const SOME_ACCEPTABLE_THRESHOLD = 10;

let iteration;

let algorithmToggle;

let pathPlanningAlgorithm = "RRT"; // Default algorithm

const stepSize = 20; // You can adjust this value based on your specific case

​

function setup() {

  createCanvas(800, 600);

  frameRate(10);

  algorithmToggle = createCheckbox("Use PRM (unchecked for RRT)", false);

  algorithmToggle.changed(() => {

    pathPlanningAlgorithm = algorithmToggle.checked() ? "PRM" : "RRT";

    newLandscape();

  });

  newLandscape();

}

​

function draw() {

  background(220);

​

  // Draw landscape

  stroke(0);

  for (let i = 0; i < landscape.length - 1; i++) {

    line(

      landscape[i].x,

      landscape[i].y,

      landscape[i + 1].x,

      landscape[i + 1].y

    );

  }

​

  // Draw start and end points

  fill(255, 0, 0);

  circle(start.x, start.y, 10);

  fill(0, 255, 0);

  circle(end.x, end.y, 10);

​

  // Draw bezier curve

  stroke(optimal ? "green" : "blue");

  noFill();

  beginShape();

  curvePoints.forEach((pt) => vertex(pt.x, pt.y));

  endShape();

​

  if (!optimal) {

    iteration(); // Perform one iteration of RRT

  } else {

    // If optimal, wait before restarting

    if (frameCount % (60 * 1.5) === 0) {

      newLandscape();

    }

  }

}

​

function newLandscape() {

  landscape = generateLandscape();

  [start, end] = generateStartEndPoints();

  curvePoints = [];

  optimal = false;

  iteration = createPathPlanningIterator(landscape, start, end);

}

​

function generateLandscape() {

  let points = [];

  for (let i = 0; i < N; i++) {

    points.push(

      createVector(

        random(width * 0.3, width * 0.7),

        random(height * 0.3, height * 0.7)

      )

    );

  }

  return points;

}

​

function generateStartEndPoints() {

  let startPoint = createVector(width * 0.2, height * 0.2).add(

    p5.Vector.random2D().mult(min(width, height) * 0.1)

  );

  let endPoint = createVector(width * 0.8, height * 0.8).add(

    p5.Vector.random2D().mult(min(width, height) * 0.1)

  );

  return [startPoint, endPoint];

}

​

function findNearest(tree, randomPoint) {

  let nearestNode = tree[0];

  let minDist = Infinity;

​

  // Iterate through all nodes in the tree to find the nearest one to the random point

  for (let node of tree) {

    let d = node.point.dist(randomPoint);

    if (d < minDist) {

      minDist = d;

      nearestNode = node;

    }

  }

​

  return nearestNode;

}

​

function steerTowards(nearestPoint, randomPoint, stepSize) {

  // Create a vector pointing from nearestPoint to randomPoint

  let direction = p5.Vector.sub(randomPoint, nearestPoint);

  // Scale the vector to the step size

  direction.setMag(stepSize);

  // Add the scaled vector to nearestPoint to get the new point in the direction of randomPoint

  let newPoint = p5.Vector.add(nearestPoint, direction);

  return newPoint;

}

​

function createRRTIterator(landscape, start, end) {

  let tree = [{ point: start, parent: null }];

  let path = [start]; // Path to visualize the steps

  let found = false; // Flag to indicate if the end point has been reached

​

  return function () {

    if (!found) {

      let randomPoint = createVector(random(width), random(height));

      let nearestNode = findNearest(tree, randomPoint);

      let newPoint = steerTowards(nearestNode.point, randomPoint, stepSize);

​

      if (isCollisionFree(nearestNode.point, newPoint, landscape)) {

        let newNode = { point: newPoint, parent: nearestNode };

        tree.push(newNode);

        path.push(newPoint);

​

        if (newPoint.dist(end) < SOME_ACCEPTABLE_THRESHOLD) {

          found = true; // Mark that the end has been reached

          path = tracePath(tree, start, end); // Trace back the path from end to start

          curvePoints = calculateBezierPoints(path);

          optimal = true; // Signal that the final path has been found

        }

      }

    }

​

    // Draw the current path

    stroke(255, 100, 100); // Red color for the path

    strokeWeight(2);

    noFill();

    beginShape();

    for (let p of path) {

      vertex(p.x, p.y);

    }

    endShape();

​

    if (found) {

      // Once the path is found, draw the final Bézier curve

      stroke("green");

      noFill();

      beginShape();

      curvePoints.forEach((pt) => vertex(pt.x, pt.y));

      endShape();

    }

  };

}

​

function isCollisionFree(pointA, pointB, landscape) {

  // Check if the line segment from pointA to pointB intersects any of the landscape lines

  for (let i = 0; i < landscape.length - 1; i++) {

    if (lineIntersects(pointA, pointB, landscape[i], landscape[i + 1])) {

      return false; // There is an intersection with an obstacle

    }

  }

  return true; // No intersections found, the path is clear

}

​

function lineIntersects(a, b, c, d) {

  // Check if line segment ab intersects with line segment cd

  const denominator = (d.y - c.y) * (b.x - a.x) - (d.x - c.x) * (b.y - a.y);

  if (denominator === 0) {

    return false; // Lines are parallel

  }

​

  const ua =

    ((d.x - c.x) * (a.y - c.y) - (d.y - c.y) * (a.x - c.x)) / denominator;

  const ub =

    ((b.x - a.x) * (a.y - c.y) - (b.y - a.y) * (a.x - c.x)) / denominator;

​

  // If both ua and ub are between 0 and 1, lines intersect

  return ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1;

}

​

// Assuming each node in the tree has a structure like: {point: createVector(x, y), parent: parentNode}

function tracePath(tree, start, end) {

  // Find the node closest to the end point

  let closestToEnd = findNearest(tree, end);

  let path = [];

  let current = closestToEnd;

​

  // Backtrack from the end node to the start node

  while (current != null && current.point != start) {

    path.push(current.point);

    current = current.parent; // Move to the parent node

  }

  path.push(start); // Add the start point at the end

​

  // Reverse the path to start from the start node to the end node

  return path.reverse();

}

​

// Calculate Bézier curve points

function calculateBezierPoints(waypoints) {

  let bezierPoints = [];

  for (let i = 0; i < waypoints.length - 3; i += 3) {

    for (let t = 0; t <= 1; t += 0.01) {

      bezierPoints.push(getBezierPoint(t, waypoints.slice(i, i + 4)));

    }

  }

  return bezierPoints;

}

​

// De Casteljau's algorithm for Bézier curve

function getBezierPoint(t, controlPoints) {

  let temp = controlPoints.map((p) => p.copy());

  let i = controlPoints.length - 1;

  while (i > 0) {

    for (let j = 0; j < i; j++) {

      temp[j] = p5.Vector.lerp(temp[j], temp[j + 1], t);

    }

    i--;

  }

  return temp[0];

}

​

// Implementations for path planning algorithms

function createPathPlanningIterator(landscape, start, end) {

  if (pathPlanningAlgorithm === "RRT") {

    return createRRTIterator(landscape, start, end);

  } else {

    return createPRMIterator(landscape, start, end);

  }

}

​