feat: pathfinding with cost

src/logic/pathfinding.js

@@ -34,7 +34,10 @@ class Pathfinding {
 
       for (const successor of this.successor(elem)) {
         if (!Pathfinding.isNodeInArr(successor, explored) && !Pathfinding.isNodeInArr(successor, fringe)) {
-          fringe.push(successor);
+          Pathfinding.insertWithPriority(successor, fringe);
+        } else if (Pathfinding.isNodeInArr(successor, fringe)) {
+          const index = Pathfinding.findIndefOf(successor, fringe);
+          fringe.splice(index, 1, successor);
         }
       }
     }
@@ -44,18 +47,38 @@ class Pathfinding {
     return !arr.every(n => !Pathfinding.reached(n.state, node.state))
   }
 
+  static findIndefOf(node, arr) {
+    return arr.findIndex(n => Pathfinding.reached(n.state, node.state));
+  }
+
+  static insertWithPriority(node, arr) {
+    for (const [i, n] of arr.entries()) {
+      if (node.totalCost < n.totalCost) {
+        console.log('Inserted inside');
+        arr.splice(i, 0, node);
+        return;
+      }
+    }
+    arr.push(node);
+  }
+
   static successor(node) {
     const list = [];
     const grid = Grid.instance.getGrid();
+    let actionCost = CostMap.instance.getCostAt(node.state.position.x, node.state.position.y);
 
     list.push({
       state: { ...node.state, rotation: (node.state.rotation + 270) % 360 },
       action: PATHFINDING_ACTION.ROTATE,
+      actionCost,
+      totalCost: Math.round((node.totalCost + actionCost) * 100) / 100 || actionCost,
       parent: node
     });
     list.push({
       state: { ...node.state, rotation: (node.state.rotation + 90) % 360 },
       action: PATHFINDING_ACTION.ROTATE,
+      actionCost,
+      totalCost: Math.round((node.totalCost + actionCost) * 100) / 100 || actionCost,
       parent: node
     });
 
@@ -63,9 +86,13 @@ class Pathfinding {
     const prediction = { x: node.state.position.x + direction.x, y: node.state.position.y + direction.y }
 
     if (grid[prediction.x] && grid[prediction.x][prediction.y] == GRID_FIELD_TYPE.PATH) {
+      actionCost = CostMap.instance.getCostAt(prediction.x, prediction.y);
+
       list.push({
         state: { ...node.state, position: prediction },
         action: PATHFINDING_ACTION.MOVE,
+        actionCost,
+        totalCost: Math.round((node.totalCost + actionCost) * 100) / 100 || actionCost,
         parent: node
       });
     }
@@ -81,9 +108,15 @@ class Pathfinding {
       result.unshift(currentElement);
       currentElement = currentElement.parent;
     }
+
+    console.log(result);
     return result;
   }
 
+  static getPriority(node, expectedNode) {
+    return Math.abs(node.state.position.x - expectedNode.state.position.x) + Math.abs(node.state.position.y - expectedNode.state.position.y);
+  }
+
   static reached(state, expectedState) {
     return state.rotation == expectedState.rotation && state.position.x == expectedState.position.x && state.position.y == expectedState.position.y;
   }

src/main.js

@@ -5,12 +5,12 @@ window.addEventListener('DOMContentLoaded', () => document.fonts.load('900 14px
   const productsCanvas = document.getElementById('canvas-products');
 	const costCanvas = document.getElementById('canvas-costmap');
 
+	const costMap = new CostMap();
+	const costMapVisualisation = new CostMapVisualisation(costCanvas);
   const floor = new Floor(floorCanvas);
   const grid = new Grid(gridCanvas);
   const agent = new Agent(agentCanvas);
   const products = new Products(productsCanvas);
-	const costMap = new CostMap();
-	const costMapVisualisation = new CostMapVisualisation(costCanvas);
 
 
 	const fpsElement = document.getElementById('fps');

src/view/grid.js

@@ -15,6 +15,7 @@ class Grid {
 
     this.setCanvasSize(canvas);
     this.processGrid();
+    this.drawCostMap();
     this.drawShelfs();
     this.drawStorages();
 
@@ -55,6 +56,18 @@ class Grid {
     }
   }
 
+  drawCostMap() {
+    for (let [x, line] of this.grid.entries()) {
+      for (let [y, type] of line.entries()) {
+        if (type === GRID_FIELD_TYPE.PATH) {
+          const cost = CostMap.instance.getCostAt(x, y);
+          this.ctx.fillStyle = `rgba(233, 241, 22, ${cost * .5})`;
+          this.ctx.fillRect(x * 100, y * 100, 100, 100);
+        }
+      }
+    }
+  }
+
   randomPlace() {
     const x = Math.floor(Math.random() * this.grid.length);
     const y = Math.floor(Math.random() * this.grid[0].length);