From 1c4e803dd7a9985485d617177e340fa6a3d88d91 Mon Sep 17 00:00:00 2001
From: Hanusiak Piotr <piotr@ltmp.co>
Date: Tue, 18 Jan 2022 12:33:46 +0100
Subject: [PATCH] move to the nearest tile if possible

---
 .../UserInput/GameSceneUserInputHandler.ts    |  2 +-
 front/src/Utils/MathUtils.ts                  |  9 +++
 front/src/Utils/PathfindingManager.ts         | 70 ++++++++++++++++++-
 3 files changed, 77 insertions(+), 4 deletions(-)

diff --git a/front/src/Phaser/UserInput/GameSceneUserInputHandler.ts b/front/src/Phaser/UserInput/GameSceneUserInputHandler.ts
index 702977cd..643b10f8 100644
--- a/front/src/Phaser/UserInput/GameSceneUserInputHandler.ts
+++ b/front/src/Phaser/UserInput/GameSceneUserInputHandler.ts
@@ -31,7 +31,7 @@ export class GameSceneUserInputHandler implements UserInputHandlerInterface {
             .getTileIndexAt(this.gameScene.CurrentPlayer.x, this.gameScene.CurrentPlayer.y);
         this.gameScene
             .getPathfindingManager()
-            .findPath(startTile, index)
+            .findPath(startTile, index, true)
             .then((path) => {
                 const tileDimensions = this.gameScene.getGameMap().getTileDimensions();
                 const pixelPath = path.map((step) => {
diff --git a/front/src/Utils/MathUtils.ts b/front/src/Utils/MathUtils.ts
index aea3bb11..c2fc88a2 100644
--- a/front/src/Utils/MathUtils.ts
+++ b/front/src/Utils/MathUtils.ts
@@ -22,4 +22,13 @@ export class MathUtils {
     public static isBetween(value: number, min: number, max: number): boolean {
         return value >= min && value <= max;
     }
+
+    public static distanceBetween(
+        p1: { x: number; y: number },
+        p2: { x: number; y: number },
+        squared: boolean = true
+    ): number {
+        const distance = Math.pow(p1.x - p2.x, 2) + Math.pow(p1.y - p2.y, 2);
+        return squared ? Math.sqrt(distance) : distance;
+    }
 }
diff --git a/front/src/Utils/PathfindingManager.ts b/front/src/Utils/PathfindingManager.ts
index 01ce5f82..2d0cae27 100644
--- a/front/src/Utils/PathfindingManager.ts
+++ b/front/src/Utils/PathfindingManager.ts
@@ -1,9 +1,11 @@
 import * as EasyStar from "easystarjs";
+import { MathUtils } from "./MathUtils";
 
 export class PathfindingManager {
     private scene: Phaser.Scene;
 
     private easyStar;
+    private grid: number[][];
 
     constructor(scene: Phaser.Scene, collisionsGrid: number[][]) {
         this.scene = scene;
@@ -11,17 +13,79 @@ export class PathfindingManager {
         this.easyStar = new EasyStar.js();
         this.easyStar.enableDiagonals();
 
-        this.setGrid(collisionsGrid);
+        this.grid = collisionsGrid;
+        this.setEasyStarGrid(collisionsGrid);
     }
 
     public async findPath(
+        start: { x: number; y: number },
+        end: { x: number; y: number },
+        tryFindingNearestAvailable: boolean = false
+    ): Promise<{ x: number; y: number }[]> {
+        let endPoints: { x: number; y: number }[] = [end];
+        if (tryFindingNearestAvailable) {
+            endPoints = [
+                end,
+                ...this.getNeighbouringTiles(end).sort((a, b) => {
+                    const aDist = MathUtils.distanceBetween(a, start, false);
+                    const bDist = MathUtils.distanceBetween(b, start, false);
+                    if (aDist > bDist) {
+                        return 1;
+                    }
+                    if (aDist < bDist) {
+                        return -1;
+                    }
+                    return 0;
+                }),
+            ];
+        }
+        let path: { x: number; y: number }[] = [];
+        while (endPoints.length > 0) {
+            const endPoint = endPoints.shift();
+            if (!endPoint) {
+                return [];
+            }
+            // rejected Promise will return undefined for path
+            path = await this.getPath(start, endPoint).catch();
+            if (path && path.length > 0) {
+                return path;
+            }
+        }
+        return [];
+    }
+
+    private getNeighbouringTiles(tile: { x: number; y: number }): { x: number; y: number }[] {
+        const xOffsets = [-1, 0, 1, 1, 1, 0, -1, -1];
+        const yOffsets = [-1, -1, -1, 0, 1, 1, 1, 0];
+
+        const neighbours: { x: number; y: number }[] = [];
+        for (let i = 0; i < 8; i += 1) {
+            const tileToCheck = { x: tile.x + xOffsets[i], y: tile.y + yOffsets[i] };
+            if (this.isTileWithinMap(tileToCheck)) {
+                neighbours.push(tileToCheck);
+            }
+        }
+        return neighbours;
+    }
+
+    private isTileWithinMap(tile: { x: number; y: number }): boolean {
+        const mapHeight = this.grid.length ?? 0;
+        const mapWidth = this.grid[0]?.length ?? 0;
+
+        return MathUtils.isBetween(tile.x, 0, mapWidth) && MathUtils.isBetween(tile.y, 0, mapHeight);
+    }
+
+    /**
+     * Returns empty array if path was not found
+     */
+    private async getPath(
         start: { x: number; y: number },
         end: { x: number; y: number }
     ): Promise<{ x: number; y: number }[]> {
         return new Promise((resolve, reject) => {
             this.easyStar.findPath(start.x, start.y, end.x, end.y, (path) => {
                 if (path === null) {
-                    reject("Path was not found");
+                    resolve([]);
                 } else {
                     resolve(path);
                 }
@@ -30,7 +94,7 @@ export class PathfindingManager {
         });
     }
 
-    private setGrid(grid: number[][]): void {
+    private setEasyStarGrid(grid: number[][]): void {
         this.easyStar.setGrid(grid);
         this.easyStar.setAcceptableTiles([0]); // zeroes are walkable
     }