// Cloned by Khema Raul on 4 Nov 2024 from World "A star (clone by Khema Raul)" by Khema Raul
// Please leave this clone trail here.
// Cloned by Khema Raul on 4 Nov 2024 from World "A star" by "Coding Train" project
// Please leave this clone trail here.
// Port of
// https://github.com/nature-of-code/NOC-S17-2-Intelligence-Learning/tree/master/week1-graphs/05_astar
// Daniel Shiffman
// Nature of Code: Intelligence and Learning
// https://github.com/shiffman/NOC-S17-2-Intelligence-Learning
// Part 1: https://youtu.be/aKYlikFAV4k
// Part 2: https://youtu.be/EaZxUCWAjb0
// Part 3: https://youtu.be/jwRT4PCT6RU
// is diagonal move allowed
const diagonal = false;
// canvas size
const cw = 900;
const ch = 600;
// How many columns and rows
// different each time
var rando = AB.randomIntAtoB(1, 5);
// var cols = 9 * rando;
// var rows = 6 * rando;
var cols = 29;
var rows = 16;
// var cols = 10;
// var rows = 10;
// how many walls to make, from 0 to 1
// different each time
// const wallAmount = AB.randomFloatAtoB(0, 0.3);
// const wallAmount = 0.2;
const wallAmount = 0.05;
const backcolor = "white";
const wallcolor = "black";
const pathcolor = "darkred";
const opencolor = "lightgreen";
const closedcolor = "lightpink";
// 2D array
var grid = new Array(cols);
// Open and closed set for both paths
var openSet1 = [];
var closedSet1 = [];
var openSet2 = [];
var closedSet2 = [];
let openSet3 = [];
let closedSet3 = [];
// let openSet4 = [];
// let closedSet4 = [];
// Start and end positions
var starts = [];
var ends = [];
// Width and height of each cell of grid
var w, h;
// The roads taken for both paths
var paths = [[], [], []]; // Two paths for the two starting positions
//=== heuristic ===========================
// this must be always optimistic - real time will be this or longer
function heuristic(a, b) {
if (diagonal) return dist(a.i, a.j, b.i, b.j);
// 2D distance
// dist is a P5 function
else return abs(a.i - b.i) + abs(a.j - b.j);
// else not diagonal, can only go across and down
// so this is optimistic
// note this is not optimistic if we can do diagonal move
}
//=== g function =======================================
// g function is known distance from start along a known path
// we work this out by adding up adjacent squares in a cumulative fashion
// note g definition should be separate from h definition (in earlier version they were confused)
function gfn(a, b) {
if (diagonal) return dist(a.i, a.j, b.i, b.j); // 2D distance
else return abs(a.i - b.i) + abs(a.j - b.j); // else go across and down
}
// Function to delete element from the array
function removeFromArray(arr, elt) {
// Could use indexOf here instead to be more efficient
for (var i = arr.length - 1; i >= 0; i--) if (arr[i] == elt) arr.splice(i, 1);
}
// Daniel Shiffman
// Nature of Code: Intelligence and Learning
// https://github.com/shiffman/NOC-S17-2-Intelligence-Learning
// Part 1: https://youtu.be/aKYlikFAV4k
// Part 2: https://youtu.be/EaZxUCWAjb0
// Part 3: https://youtu.be/jwRT4PCT6RU
// An object to describe a spot in the grid
function Spot(i, j) {
// Location
this.i = i;
this.j = j;
// f, g, and h values for A*
this.f = 0;
this.g = 0;
this.h = 0;
// Neighbors
this.neighbors = [];
// Where did I come from?
this.previous = undefined;
// Am I an wall?
if (random(1) < wallAmount) this.wall = true;
else this.wall = false;
// Display me
this.show = function (col) {
if (this.wall) {
fill(wallcolor);
noStroke();
// wall fills square
rect(this.i * w, this.j * h, w, h);
// wall only partially fills square
// ellipse ( this.i * w + w / 2, this.j * h + h / 2, w * 0.7, h * 0.7 );
} else if (col) {
fill(col);
rect(this.i * w, this.j * h, w, h);
}
};
// Figure out who my neighbors are
this.addNeighbors = function (grid) {
var i = this.i;
var j = this.j;
if (i < cols - 1) this.neighbors.push(grid[i + 1][j]);
if (i > 0) this.neighbors.push(grid[i - 1][j]);
if (j < rows - 1) this.neighbors.push(grid[i][j + 1]);
if (j > 0) this.neighbors.push(grid[i][j - 1]);
if (diagonal) {
// diagonals are also neighbours:
if (i > 0 && j > 0) this.neighbors.push(grid[i - 1][j - 1]);
if (i < cols - 1 && j > 0) this.neighbors.push(grid[i + 1][j - 1]);
if (i > 0 && j < rows - 1) this.neighbors.push(grid[i - 1][j + 1]);
if (i < cols - 1 && j < rows - 1) this.neighbors.push(grid[i + 1][j + 1]);
}
};
}
function setup() {
// slower frame rate to see how it is working
// frameRate (4);
createCanvas(cw, ch);
// Grid cell size
w = width / cols;
h = height / rows;
// Making a 2D array
for (var i = 0; i < cols; i++) grid[i] = new Array(rows);
for (var i = 0; i < cols; i++)
for (var j = 0; j < rows; j++) grid[i][j] = new Spot(i, j);
// All the neighbors
for (var i = 0; i < cols; i++)
for (var j = 0; j < rows; j++) grid[i][j].addNeighbors(grid);
// Set start positions
const start1 = grid[0][0]; // Top-left
const start2 = grid[cols-1][0]; // Close to start1
const start3 = grid[12][0]; // Close to start1
// const start4 = grid[2][2]; // Close to start1 and start3
start1.wall = false; // Ensure start is not a wall
start2.wall = false; // Ensure second start is not a wall
start3.wall = false; // Ensure start is not a wall
// start4.wall = false; // Ensure second start is not a wall
starts.push(start1, start2, start3); // Store start positions in the starts array
// Set end positions
const end1 = grid[cols - 1][rows - 1]; // Bottom-right
const end2 = grid[0][4]; // Close to end1
const end3 = grid[cols - 12][rows - 1]; // Close to end1
// const end4 = grid[cols - 2][rows - 2]; // Close to end1 and end3
end1.wall = false; // Ensure end is not a wall
end2.wall = false; // Ensure second end is not a wall
end3.wall = false; // Ensure end is not a wall
// end4.wall = false; // Ensure second end is not a wall
ends.push(end1, end2, end3); // Store end positions in the ends array
// Open set starts with both beginnings
openSet1.push(start1);
openSet2.push(start2);
openSet3.push(start3);
// openSet4.push(start4);
console.log("Start search");
}
let currentPathIndex = [0, 0, 0]; // Start index for both paths
let pathFound = [false, false, false]; // Flags to check if paths are complete
let t = [0, 0, 0]; // Interpolation factors for both paths
let speed = 0.05; // Controls the speed of the car
// New flag to stop car2
let isCarStopped = [false, false, false]; // Car stop flags (for car1 and car2)
function draw() {
// Only process one path per frame based on path completion flags
if (!pathFound[0]) {
processPathfinding(openSet1, closedSet1, ends[0], 0);
} else if (!pathFound[1]) {
processPathfinding(openSet2, closedSet2, ends[1], 1);
} else if (!pathFound[2]) {
processPathfinding(openSet3, closedSet3, ends[2], 2);
}
// else if (!pathFound[3]) {
// processPathfinding(openSet4, closedSet4, ends[3], 3);
// }
// Draw grid and sets
background(backcolor);
for (var i = 0; i < cols; i++) {
for (var j = 0; j < rows; j++) {
grid[i][j].show();
}
}
for (var i = 0; i < closedSet1.length; i++) {
closedSet1[i].show(closedcolor);
}
for (var i = 0; i < openSet1.length; i++) {
openSet1[i].show(opencolor);
}
for (var i = 0; i < closedSet2.length; i++) {
closedSet2[i].show(closedcolor);
}
for (var i = 0; i < openSet2.length; i++) {
openSet2[i].show(opencolor);
}
for (var i = 0; i < closedSet3.length; i++) {
closedSet3[i].show(closedcolor);
}
for (var i = 0; i < openSet3.length; i++) {
openSet3[i].show(opencolor);
}
// for (var i = 0; i < closedSet4.length; i++) {
// closedSet4[i].show(closedcolor);
// }
// for (var i = 0; i < openSet4.length; i++) {
// openSet4[i].show(opencolor);
// }
starts[0].show("blue"); // Color start of path 1
starts[1].show("blue"); // Color start of path 2
starts[2].show("orange"); // Color start of path 1
// starts[3].show("orange"); // Color start of path 2
ends[0].show("orange"); // Color end of path 1
ends[1].show("orange"); // Color end of path 2
ends[2].show("blue"); // Color end of path 1
// ends[3].show("blue"); // Color end of path 2
if (pathFound[0] && pathFound[1] && pathFound[2]){
animateCars();
}
}
// Modify the processPathfinding function to be async
function processPathfinding(openSet, closedSet, end, pathIndex) {
// Early exit if the path has already been found
if (pathFound[pathIndex]) {
return; // Skip processing if the path is already found
}
if (openSet.length > 0) {
// Find the best next option
var winner = 0;
for (var i = 0; i < openSet.length; i++) {
if (openSet[i].f < openSet[winner].f) {
winner = i;
}
}
var current = openSet[winner];
// Check if the current node is the end node
if (current === end) {
pathFound[pathIndex] = true; // Mark path as found
console.log(`Path ${pathIndex + 1} found!`);
// Backtrack to create path array
let temp = current;
paths[pathIndex].push(temp);
while (temp.previous) {
paths[pathIndex].push(temp.previous);
temp = temp.previous;
}
paths[pathIndex].reverse(); // Reverse the path for correct order
}
// Remove current from openSet and add to closedSet
removeFromArray(openSet, current);
closedSet.push(current);
// Loop through neighbors
let neighbors = current.neighbors;
for (let i = 0; i < neighbors.length; i++) {
let neighbor = neighbors[i];
// Only proceed if neighbor is not in closedSet and not a wall
if (!closedSet.includes(neighbor) && !neighbor.wall) {
let gScore = current.g + gfn(current, neighbor);
let gScoreIsBest = false;
if (!openSet.includes(neighbor)) {
openSet.push(neighbor); // Discover a new node
gScoreIsBest = true;
} else if (gScore < neighbor.g) {
gScoreIsBest = true; // We're already in openSet, but this is a better path
}
if (gScoreIsBest) {
neighbor.previous = current;
neighbor.g = gScore;
neighbor.h = heuristic(neighbor, end);
neighbor.f = neighbor.g + neighbor.h;
}
}
}
}
}
let collisionCell = null; // To track the collision cell
let hasMovedBack = false;
function animateCars() {
// Check if paths have been found for all four cars
if (pathFound[0] && pathFound[1] && pathFound[2]) {
// Collision detection and handling for each car with every other car
for (let p = 0; p < paths.length; p++) {
for (let q = p + 1; q < paths.length; q++) {
if (currentPathIndex[p] < paths[p].length - 1 && currentPathIndex[q] < paths[q].length - 1) {
let carPosP = paths[p][currentPathIndex[p]];
let carPosQ = paths[q][currentPathIndex[q]];
let nextCarPosP = paths[p][currentPathIndex[p] + 1];
let nextCarPosQ = paths[q][currentPathIndex[q] + 1];
// Detect collision if both cars are at the same position or moving to the same next position
if ((carPosP.i === carPosQ.i && carPosP.j === carPosQ.j) ||
(nextCarPosP.i === nextCarPosQ.i && nextCarPosP.j === nextCarPosQ.j)) {
console.log(`Collision detected between Car ${p+1} and Car ${q+1}`);
// Stop both cars if a collision is detected
isCarStopped[p] = true;
isCarStopped[q] = true;
// Move each car back if possible
if (currentPathIndex[q] > 0) {
currentPathIndex[q]--; // Move car q back by one step
hasMovedBack[q] = true; // Track move back for car q individually
}
if (currentPathIndex[p] > 0) {
currentPathIndex[p]--; // Move car p back by one step
hasMovedBack[p] = true; // Track move back for car p individually
}
}
}
}
}
for (let p = 0; p < paths.length; p++) {
let path = paths[p];
// Continue animating if we haven't reached the last path node
if (!isCarStopped[p] && currentPathIndex[p] === path.length - 1) {
let finalPos = path[currentPathIndex[p]];
let carX = finalPos.i * w + w / 2;
let carY = finalPos.j * h + h / 2;
let angle = 0;
// Draw the path
noFill();
stroke(pathcolor);
strokeWeight(w / 8);
beginShape();
for (let i = 0; i < path.length; i++) {
vertex(path[i].i * w + w / 2, path[i].j * h + h / 2);
}
endShape();
// Draw the car at the last position
drawCar(carX, carY, angle, "red");
} else if (!isCarStopped[p] && currentPathIndex[p] < path.length) {
let currentPos = path[currentPathIndex[p]];
let nextPos = currentPathIndex[p] < path.length - 1 ? path[currentPathIndex[p] + 1] : currentPos;
// Interpolated position
let carX = lerp(currentPos.i * w + w / 2, nextPos.i * w + w / 2, t[p]);
let carY = lerp(currentPos.j * h + h / 2, nextPos.j * h + h / 2, t[p]);
let angle = atan2(nextPos.j - currentPos.j, nextPos.i - currentPos.i);
// Draw the path up to the current point
noFill();
stroke(pathcolor);
strokeWeight(w / 8);
beginShape();
for (let i = 0; i <= currentPathIndex[p]; i++) {
vertex(path[i].i * w + w / 2, path[i].j * h + h / 2);
}
endShape();
// Draw the car
drawCar(carX, carY, angle, "red");
// Update car's position
t[p] += speed;
if (t[p] >= 1) {
t[p] = 0;
currentPathIndex[p]++;
}
} else {
// If the car is stopped, keep it at its last position (do not move it further)
let currentPos = path[currentPathIndex[p]];
let carX = currentPos.i * w + w / 2;
let carY = currentPos.j * h + h / 2;
let angle = atan2(path[currentPathIndex[p] + 1].j - currentPos.j, path[currentPathIndex[p] + 1].i - currentPos.i);
// Draw the path up to the current point
noFill();
stroke(pathcolor);
strokeWeight(w / 8);
beginShape();
for (let i = 0; i <= currentPathIndex[p]; i++) {
vertex(path[i].i * w + w / 2, path[i].j * h + h / 2);
}
endShape();
// Draw the car at the current position
drawCar(carX, carY, angle, "red");
console.log(`Car ${p + 1} is stopped at (${carX}, ${carY})`);
// Check if cars can resume after moving back
if (isCarStopped[p] && hasMovedBack[p]) {
let carPos = paths[p][currentPathIndex[p]];
// Check if any other stopped car is at the same position or directly in the way
let canResume = true;
for (let q = 0; q < paths.length; q++) {
if (p !== q && isCarStopped[q]) {
let otherCarPos = paths[q][currentPathIndex[q]];
// If another car is in the same or next position, car `p` should stay stopped
if ((carPos.i === otherCarPos.i && carPos.j === otherCarPos.j) ||
(paths[p][currentPathIndex[p] + 1] && paths[q][currentPathIndex[q] + 1] &&
paths[p][currentPathIndex[p] + 1].i === paths[q][currentPathIndex[q] + 1].i &&
paths[p][currentPathIndex[p] + 1].j === paths[q][currentPathIndex[q] + 1].j)) {
canResume = false;
break;
}
}
}
// Resume movement if no collision risk is detected
if (canResume) {
console.log(`Car ${p + 1} resumes movement`);
isCarStopped[p] = false;
hasMovedBack[p] = false; // Reset move-back status
}
}
}
}
}
if (pathFound.every((pf, i) => pf && currentPathIndex[i] >= paths[i].length - 1)) {
noLoop();
}
}
// Function to draw a car at a given position with a given color
function drawCar(x, y, angle, color) {
push();
translate(x, y);
rotate(angle);
fill(color);
noStroke();
rectMode(CENTER);
rect(0, 0, w * 0.6, h * 0.3);
// Wheels
fill("black");
let wheelOffsetX = w / 4;
let wheelOffsetY = h / 4;
ellipse(-wheelOffsetX, wheelOffsetY, w / 8, h / 8);
ellipse(wheelOffsetX, wheelOffsetY, w / 8, h / 8);
pop();
}