// 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 = 9;
var rows = 6;
// how many walls to make, from 0 to 1
// different each time
// const wallAmount = AB.randomFloatAtoB(0, 0.3);
const wallAmount = 0.25;
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 = [];
// 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]; // Start at top-left
const start2 = grid[cols - 1][0]; // Start at top-right
start1.wall = false; // Ensure start is not a wall
start2.wall = false; // Ensure second start is not a wall
starts.push(start1, start2); // Store start positions in the starts array
// Set end positions
const end1 = grid[cols - 1][rows - 1]; // End at bottom-left
const end2 = grid[0][rows - 1]; // End at bottom-right
end1.wall = false; // Ensure end is not a wall
end2.wall = false; // Ensure second end is not a wall
ends.push(end1, end2); // Store end positions in the ends array
// Open set starts with both beginnings
openSet1.push(start1);
openSet2.push(start2);
console.log("Start search");
}
let currentPathIndex = [0, 0]; // Start index for both paths
let pathFound = [false, false]; // Flags to check if paths are complete
let t = [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]; // 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);
}
// 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);
}
starts[0].show("blue"); // Color start of path 1
starts[1].show("blue"); // Color start of path 2
ends[0].show("orange"); // Color end of path 1
ends[1].show("orange"); // Color end of path 2
animateCars();
}
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 both paths have been found
if (pathFound[0] && pathFound[1]) {
for (let p = 0; p < paths.length; p++) {
let path = paths[p];
if (currentPathIndex[0] < paths[0].length - 2 && currentPathIndex[1] < paths[1].length - 2) {
let nextCar1Pos = paths[0][currentPathIndex[0] + 2];
let nextCar2Pos = paths[1][currentPathIndex[1] + 2];
// Check for potential collision
if (nextCar1Pos.i === nextCar2Pos.i && nextCar1Pos.j === nextCar2Pos.j) {
console.log("Potential collision detected","===Collsion-detected===",isCarStopped[1]);
/// Stop car2 if collision detected
if (!isCarStopped[1]) {
console.log("next-pos-check",nextCar1Pos.j, nextCar2Pos.j)
// Check if the cars are in the same row
if (nextCar1Pos.j === nextCar2Pos.j) {
console.log("Cars are in the same row! Stopping car2 at row -1");
// Stop car2 at the row before the collision row
let stopRow = nextCar2Pos.j - 1;
isCarStopped[1] = true; // Stop car2
collisionCell = {i: nextCar2Pos.i, j: stopRow}; // Set the stop row for car2
console.log("Car 2 stopped at row:", stopRow);
} else {
isCarStopped[1] = true; // Stop car2 normally if not in the same row
}
}
}
}
// 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
push();
translate(carX, carY);
rotate(angle);
fill("blue");
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();
} 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
push();
translate(carX, carY);
rotate(angle);
fill("red");
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();
// 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();
console.log("HAS-MOVED-BACK",hasMovedBack);
// Code to handle the car movement and checking for left turn
if (collisionCell && p === 1 && !hasMovedBack) {
// Start moving backward until we find a left turn
let foundLeftTurn = false;
// Check if there are at least two positions available in the path before the collision
while (currentPathIndex[p] > 0 && !foundLeftTurn) {
// Check if the current position forms a left turn
if (isLeftTurn(path, currentPathIndex[p])) {
console.log("Left turn detected at index " + currentPathIndex[p] + ", stopping.");
foundLeftTurn = true; // Stop when a left turn is found
} else {
console.log("No left turn detected, moving back one step.");
currentPathIndex[p] = currentPathIndex[p] - 1; // Move back one step in the path
}
}
// If a left turn is found, stop moving back
if (foundLeftTurn) {
hasMovedBack = true; // Indicate that we've found the left turn and stopped moving back
}
}
// Draw the car at the current position
push();
translate(carX, carY);
rotate(angle);
fill("red");
noStroke();
rectMode(CENTER);
rect(0, 0, w * 0.6, h * 0.3);
// Draw 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();
// Make sure the car is not moving further
console.log(`Car ${p + 1} is stopped at (${carX}, ${carY})`);
// Check if car1 has passed the collision point (if car1's position goes beyond the collision cell)
if (collisionCell && isCarStopped[1]) {
let car1Pos = paths[0][currentPathIndex[0]];
let car2Pos = paths[1][currentPathIndex[1]];
// console.log("CAR-POSITIONS-COLLISION-CELL",car1Pos.i,collisionCell.i,car1Pos.j,collisionCell.j,car1Pos.i === collisionCell.i,car1Pos.i > collisionCell.i,car1Pos.j > collisionCell.j);
// Check if Car 1 has passed the collision cell (either in i or j direction)
if (car1Pos.i - 1 > collisionCell.i || car1Pos.j - 1 > collisionCell.j) {
isCarStopped[1] = false; // Resume car2's movement
}
}
}
}
}
if (pathFound[0] && pathFound[1] && currentPathIndex[0] >= paths[0].length - 1 && currentPathIndex[1] >= paths[1].length - 1) {
noLoop();
}
}
function isLeftTurn(path, currentIndex) {
// Ensure we have enough points to check the turn (previous, current, and next positions)
if (currentIndex <= 0 || currentIndex >= path.length - 1) return false;
let prevPos = path[currentIndex - 1];
let currPos = path[currentIndex];
let nextPos = path[currentIndex + 1];
// Calculate angles for the previous to current segment and current to next segment
let angle1 = atan2(currPos.j - prevPos.j, currPos.i - prevPos.i);
let angle2 = atan2(nextPos.j - currPos.j, nextPos.i - currPos.i);
// Calculate the difference between the two angles
let angleDiff = (angle2 - angle1) * (180 / Math.PI); // Convert angle difference to degrees
// If the angle difference is negative and exceeds a threshold, it's a left turn
return angleDiff < -45; // Adjust this threshold as needed
}
// Helper function to recalculate path for a specific car
function recalculatePath(carIndex) {
// Reset pathfinding state for this car
pathFound[carIndex] = false;
currentPathIndex[carIndex] = 0;
paths[carIndex] = [];
// Restart the pathfinding process for the specified car
if (carIndex === 0) {
openSet1 = [starts[0]];
closedSet1 = [];
} else {
openSet2 = [starts[1]];
closedSet2 = [];
}
}