//------kexin Ma modify start--------
// canvas size
const cw = 1000;
const ch = 600;
// How many columns and rows
// different each time
var rando = AB.randomIntAtoB(1, 4);
var cols = 8 * rando;
var rows = 6 * rando;
// how many walls to make, from 0 to 1
// different each time
const wallAmount = AB.randomFloatAtoB(0, 0.4);
// MH edit
//background color
const backcolor = 'white'; // '#9f9999';
//path color
const pathcolor = 'darkred'; // 'green';
//search open color
const opencolor = 'lightgreen'; // '#9f9999';
//search closr color
const closedcolor = 'lightpink'; // '#999393';
//------kexin Ma modify end--------
// 2D array
var grid = new Array(cols);
// Open and closed set
var openSet = [];
var closedSet = [];
//------kexin Ma add start-------
//Open and closed set array (4 cars)
var startPoints = [];
var endPoints = [];
//------kexin Ma add end--------
// Start and end
var start;
var end;
// Width and height of each cell of grid
var w, h;
//------kexin Ma add start--------
// The road taken, because it has 4 paths, in order to distinguish path
var path_k = [[], []];
// flags, each path has found the path
var allPathsFind_flag = [false, false, false, false];
//random priority
var collisionCounter = 0;
//------kexin Ma add end--------
//=== heuristic ===========================
// this must be always optimistic - real time will be this or longer
function heuristic(a, b) {
return (abs(a.i - b.i) + abs(a.j - b.j));
}
//=== 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) {
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);
}
}
}
// 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_k = undefined;
this.previous_k = [];
// Am I an wall?
if (random(1) < wallAmount) this.wall = true;
else this.wall = false;
//------kexin Ma add start--------
//display the best path
this.showPath = function(current,k) {
//draw a car
//different paths display different colored cars
//different size can show all the cars at the same point
var color_rect="";
if(k==0){
color_rect="#b62525";
w_rect=w * 0.9;
}else if(k==1){
color_rect="#ee510b";
w_rect=w * 0.86;
}else if(k==2){
color_rect="#0bd6ed";
w_rect=w * 0.82;
}else{
color_rect="#eb50cc";
w_rect=w * 0.78;
}
//car body
fill(color_rect);
rect(this.i * w+w*0.05, this.j * h+h * 0.35, w_rect, h * 0.35);
rect(this.i * w+w*0.25, this.j * h+h * 0.05, w_rect*0.5, h * 0.35);
//wheels
fill(0);
ellipse(this.i * w + w / 4, this.j * h + h * 0.7, w * 0.25, h * 0.25);
fill("white");
ellipse(this.i * w + w / 4, this.j * h + h * 0.7, w * 0.07, h * 0.07);
fill(0);
ellipse(this.i * w + w * 0.65, this.j * h + h * 0.7, w * 0.25, h * 0.25);
fill("white");
ellipse(this.i * w + w * 0.65, this.j * h + h * 0.7, w * 0.07, h * 0.07);
// window
fill("white");
rect(this.i * w+w*0.45, this.j * h+h * 0.15, w_rect*0.15, h * 0.15);
//draw the destination and start
//add the text "end path" to show the destination
if (this === endPoints[k]) {
//follow the car color
fill(color_rect);
textSize(12);
textAlign(CENTER, CENTER);
//k stands for the path
//"end k" text above of the cars
text("end"+k, this.i * w + w *0.2 , this.j * h);
}
//add the text "start path" to show the start
if (this === startPoints[k]) {
//follow the car color
fill(color_rect);
textSize(12);
textAlign(CENTER, CENTER);
//k stands for the path
//"start k" text bottom of the cars
text("start"+k, this.i * w + w *0.2 , this.j * h + h*0.85);
}
}
// display the wall, gird, the search start and close
this.show = function(col) {
if (this.wall) {
//draw a house
//roof
fill("#c48474");
noStroke();
triangle(this.i * w + w * 0.5, this.j * h, this.i * w + w * 0.05, this.j * h + h * 0.3, this.i * w + w * 0.95, this.j * h + h * 0.3);
//body
fill("#c9bb89");
noStroke();
rect(this.i * w + w * 0.05, this.j * h + h * 0.3, w * 0.90, h * 0.65);
//window
fill("#fdfaf9");
noStroke();
rect(this.i * w + w * 0.15, this.j * h + h * 0.4, w * 0.15, h * 0.2);
//door
fill("#b1a383");
noStroke();
rect(this.i * w + w * 0.35, this.j * h + h * 0.65, w * 0.30, h * 0.3);
} else if (col) {
//display gird, the search start and close
fill(col);
rect(this.i * w, this.j * h, w, h);
}
};
//------kexin Ma add end--------
// 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]);
}
}
function setup() {
createCanvas(cw, ch);
//------kexin Ma add start--------
//avoid refreshing the canvas
//the paths of the four cars can be displayed
background(backcolor);
//------kexin Ma add end--------
// 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);
}
}
//------kexin Ma add start--------
// randomly create 4 pairs of desti and start points
var temRan_start=[];
var temRan_end=[];
//create 4 pairs
for (let i = 0; i < 4; i++) {
var rando_start = AB.randomIntAtoB(0, rows - 1);
//avoid create the same start points
while(temRan_start.includes(rando_start)){
rando_start = AB.randomIntAtoB(0, rows - 1);
}
temRan_start.push(rando_start);
//avoid create the same desti points
var rando_end = AB.randomIntAtoB(0, cols - 1);
while(temRan_end.includes(rando_end)){
rando_end = AB.randomIntAtoB(0, cols - 1);
}
temRan_end.push(rando_end);
//start point at the random position in the first line
let start = grid[rando_start][0];
start.wall = false;
//push the value into array
startPoints.push(start);
//desti point at the random position in the last line
let end = grid[rando_end][rows - 1];
end.wall = false;
endPoints.push(end);
//show the position in each path
var b=rows - 1;
console.log("---start--"+i+"---->"+rando_start+",0"+"--end--"+i+"---->"+rando_end+","+b);
}
// Initialize openSet and closedSet
for (let i = 0; i < 4; i++) {
openSet.push([startPoints[i]]);
closedSet.push([]);
}
//------kexin Ma add end--------
console.log('start search');
}
function draw() {
// MH edit
frameRate (3);
//Draw grid[i][j]
for (var i = 0; i < cols; i++){
for (var j = 0; j < rows; j++){
grid[i][j].show();
}
}
// the search goes on over many timesteps
// each timestep, check one more square and draw current partial solution
//------kexin Ma modify start--------
//4 paths
for (let k = 0; k < 4; k++) {
// if this path find the path , continue next path
if (allPathsFind_flag[k]) {
continue;
}
if (openSet[k].length > 0) {
// --- begin still searching -----------------------------
// Best next option
var winner = 0;
//openSet[k] is one path
for (var i = 0; i < openSet[k].length; i++) {
//openSet[k][i] is a point
if (openSet[k][i].f < openSet[k][winner].f) {
winner = i;
}
}
// this path's best point
var current = openSet[k][winner];
// Did I finish?
if (current === endPoints[k]) {
//set a flag to stand for has found this path
allPathsFind_flag[k] = true;
console.log("success - found path for path", k);
}
// Best option moves from openSet to closedSet
removeFromArray(openSet[k], current);
closedSet[k].push(current);
// Check all the neighbors
var neighbors = current.neighbors;
//--- start of for loop -----------
for (var i = 0; i < neighbors.length; i++) {
var neighbor = neighbors[i];
// Valid next spot?
if (!closedSet[k].includes(neighbor) && !neighbor.wall) {
var g = current.g + gfn(neighbor, current); // add up g values in cumulative way
// Is this a better path than before?
var newPath = false;
if (openSet[k].includes(neighbor)) {
if (g < neighbor.g) {
neighbor.g = g;
newPath = true;
}
} else {
neighbor.g = g;
newPath = true;
openSet[k].push(neighbor);
}
// Yes, it's a better path
if (newPath) {
neighbor.h = heuristic(neighbor, endPoints[k]);
neighbor.f = neighbor.g + neighbor.h;
neighbor.previous_k[k] = current;
}
}
}
//--- end of for loop -----------
}
//--- end still searching -----------------------------
else {
console.log("fail - no path exists for path", k);
// although it failed, flag was set true to avoid repeatedly searching
allPathsFind_flag[k] = true;
continue;
}
for (var i = 0; i < closedSet[k].length; i++) {
closedSet[k][i].show(closedcolor);
}
for (let i = 0; i < openSet[k].length; i++) {
openSet[k][i].show(opencolor);
}
// Find the path by working backwards
//path_k[k] is this path
path_k[k] = [];
let temp = current;
//avoid undefined
if (temp) {
path_k[k].push(temp);
//avoid undefined
while (temp.previous_k[k]) {
if (path_k[k].includes(temp.previous_k[k])) {
console.error("temp.previous undefined");
break;
}
path_k[k].push(temp.previous_k[k]);
temp = temp.previous_k[k];
}
}
// path as cars
for (var i = 0; i < path_k[k].length; i++){
path_k[k][i].showPath(current,k); // Call the custom show method
}
}
// draw all the paths found to avoid missing any
for (let k = 0; k < 4; k++) {
for (var i = 0; i < path_k[k].length; i++) {
path_k[k][i].showPath(current, k);
}
}
//------kexin Ma modify end--------
//------kexin Ma add start---------
// check Collisions
checkCollisions();
//check all paths have been completed
if (allPathsFind_flag.every(found => found)) {
// end the search
noLoop();
}
}
// check Collisions
function checkCollisions() {
// record the position that current point
let currentPositions = [];
//4 paths
for (let q = 0; q < 4; q++) {
if (allPathsFind_flag[q]) {
// this path has found, continue
continue;
}
// this path has not found
if (path_k[q].length > 0) {
// the current point in this path
let currentPoint = path_k[q][0];
//get postition
let postition = `${currentPoint.i}-${currentPoint.j}`;
//set postition and index into array
currentPositions.push({ pathIndex: q, pos: postition });
}
}
// record the count of collision paths at the same point
let positionNums = {};
currentPositions.forEach(item => {
//if it contains the pos, push an index into positionNums in this position
if (positionNums[item.pos]) {
positionNums[item.pos].push(item.pathIndex);
} else {
//if it not contains the pos, push an object into positionNums
positionNums[item.pos] = [item.pathIndex];
}
});
//judge of collision
for (let pos in positionNums) {
//collision
if (positionNums[pos].length > 1) {
//record this position and paths
console.log("---Collision position--->", pos, "---paths--->", positionNums[pos]+random(100));
let collidingPathPos = positionNums[pos];
//handle collision
handleCollision(collidingPathPos);
}
}
}
//handle collision
function handleCollision(collidingPathPos) {
//priority
//random a index
let primaryPathIndex = collisionCounter % collidingPathPos.length;
//get the primary path
let primaryPath = collidingPathPos[primaryPathIndex];
// handle collision
collidingPathPos.forEach((pathIndex, idx) => {
//not the primary path
if (idx !== primaryPathIndex) {
//get the collision node
let collisionNode = path_k[primaryPath][0];
// increase the g at the collision node to avoid other path collisions it
collisionNode.g += 15;
//allow the non-primary path to continue to find
allPathsFind_flag[pathIndex] = false;
// non-primary path to be re-planned
setTimeout(() => {
// Reset openSet at the new startpoint
openSet[pathIndex] = [startPoints[pathIndex]];
//Clear closedSet
closedSet[pathIndex] = [];
}, 500);
}
});
// change priority to avoid not finding the best path
collisionCounter++;
}
//------kexin Ma add end--------