searchPath.test.ts

import assert from "assert";
import searchPath, { Point } from "./searchPath.ts";
import range from "lodash/range";

type Hitmap = {
  sizeX: number;
  sizeY: number;
  map: number[];
};

function hitByGrid(hitmap: Hitmap, { x, y }: Point): boolean {
  if (x < 0 || hitmap.sizeX - 1 < x || y < 0 || hitmap.sizeY - 1 < y) {
    return true;
  }
  return !!hitmap.map[~~y * hitmap.sizeX + ~~x];
}

function createBlankHitmapWithWall(sizeX: number, sizeY: number) {
  return {
    map: range(sizeX * sizeY).map(idx => {
      return (
        // left
        idx % sizeX === 0 ||
          // right
          idx % sizeX === sizeX - 1 ||
          // up
          ~~(idx / sizeX) === 0 ||
          // bottom
          ~~(idx / sizeX) === sizeY - 1
          ? 1
          : 0
      );
    }),
    sizeX,
    sizeY
  };
}

test("#searchPath can pass to neighbor", () => {
  const hitmap = createBlankHitmapWithWall(3, 4);
  const hitFunc = (point: Point) => {
    return hitByGrid(hitmap, point);
  };

  const start = { x: 1, y: 1 };
  const goal = { x: 1, y: 2 };
  assert(!hitByGrid(hitmap, start));
  assert(!hitByGrid(hitmap, goal));
  const ret = searchPath(start, goal, hitFunc);
  assert.deepEqual(ret, [goal]);
});

test("#searchPath can not pass to neighbor", () => {
  const hitmap = createBlankHitmapWithWall(3, 3);
  const hitFunc = (point: Point) => {
    return hitByGrid(hitmap, point);
  };
  const start = { x: 1, y: 1 };
  const goal = { x: 1, y: 2 };
  assert(!hitByGrid(hitmap, start));
  assert(hitByGrid(hitmap, goal));
  const ret = searchPath(start, goal, hitFunc);
  assert(ret == null);
});

test("#searchPath can pass to next of next", () => {
  const hitmap = createBlankHitmapWithWall(3, 5);
  const hitFunc = (point: Point) => {
    return hitByGrid(hitmap, point);
  };

  const start = { x: 1, y: 1 };
  const goal = { x: 1, y: 3 };
  assert(!hitByGrid(hitmap, start));
  assert(!hitByGrid(hitmap, goal));
  const ret = searchPath(start, goal, hitFunc);
  assert.deepEqual(ret, [{ x: 1, y: 2 }, goal]);
});

test("searchPath: slant with blocked neighbor", () => {
  const hitmap = createBlankHitmapWithWall(3, 3);
  // prettier-ignore
  hitmap.map = [
    1, 1, 1,
    1, 0, 1,
    1, 1, 0
  ]
  const start = { x: 1, y: 1 };
  const goal = { x: 2, y: 2 };
  assert(!hitByGrid(hitmap, start));
  assert(!hitByGrid(hitmap, goal));

  const hitFunc = (point: Point) => {
    return hitByGrid(hitmap, point);
  };
  const ret = searchPath(start, goal, hitFunc);
  assert(ret == null);
});

test("searchPath: slant with open neighbor", () => {
  const hitmap = createBlankHitmapWithWall(3, 3);
  // prettier-ignore
  hitmap.map = [
    1, 1, 1,
    1, 0, 0,
    1, 1, 0
  ]
  const start = { x: 1, y: 1 };
  const goal = { x: 2, y: 2 };
  assert(!hitByGrid(hitmap, start));
  assert(!hitByGrid(hitmap, goal));
  // const ret = hitByGrid(hitmap, { x: 1, y: 1 })

  const hitFunc = (point: Point) => {
    return hitByGrid(hitmap, point);
  };
  const ret = searchPath(start, goal, hitFunc);
  assert.deepEqual(ret, [{ x: 2, y: 1 }, goal]);
});

searchPath.ts

import differenceBy from "lodash/differenceBy";
import compact from "lodash/compact";
import minBy from "lodash/minBy";

export type Point = {
  x: number;
  y: number;
};

export type Node = {
  pos: Point;
  cost: number;
  parent: Node | null;
};

// prettier-ignore
const SEARCHING_DIRS: Array<[number, number]> = [
  [-1, -1], [ 0, -1], [+1, -1],
  [-1,  0],           [+1,  0],
  [-1, +1], [ 0, +1], [+1, +1]
]

const calcCost = (p1: Point, p2: Point) => {
  return (p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2;
};

type HitFunc = (g: Point) => boolean;

const searchNeibors = (
  current: Node,
  goal: Point,
  hitFunc: HitFunc
): Node[] => {
  const openNeighbors = compact(
    SEARCHING_DIRS.map(([dx, dy]) => {
      const ng: Point = {
        x: current.pos.x + dx,
        y: current.pos.y + dy
      };
      if (!hitFunc(ng)) {
        // slant
        if ((dx + dy) % 2 === 0) {
          if (
            // blocked by both neighbor
            hitFunc({ x: ng.x, y: current.pos.y }) &&
            hitFunc({ x: current.pos.x, y: ng.y })
          ) {
            return null;
          } else if (
            // blocked by x-axis
            !hitFunc({ x: ng.x, y: current.pos.y }) &&
            hitFunc({ x: current.pos.x, y: ng.y })
          ) {
            const pos: Point = { x: ng.x, y: current.pos.y };
            return {
              pos,
              cost: calcCost(pos, goal),
              parent: current
            };
          } else if (
            // blocked by y-axis
            hitFunc({ x: ng.x, y: current.pos.y }) &&
            !hitFunc({ x: current.pos.x, y: ng.y })
          ) {
            const pos = { x: current.pos.x, y: ng.y };
            return {
              pos,
              cost: calcCost(pos, goal),
              parent: current
            };
          }
        }
        // Go straight or not blocked neither both
        return {
          pos: ng,
          cost: calcCost(ng, goal),
          parent: current
        };
      } else {
        return null;
      }
    })
  );
  return differenceBy(
    openNeighbors,
    // @ts-ignore
    (node: Node) => `${node.pos.x}:${node.pos.y}`
  );
};

const selectMinCostNode = (nodes: Node[]): Node | void => {
  if (nodes.length === 0) {
    return undefined;
  }
  return minBy(nodes, n => n.cost);
};

const resolvePathFromGoalToStart = (goal: Node): Point[] => {
  const result = [goal.pos];
  let node: Node | null = goal;
  while ((node = node.parent)) {
    result.push(node.pos);
  }
  // 1st route is it self cell
  return result.reverse().slice(1);
};

export default (
  start: Point,
  goal: Point,
  hitFunc: HitFunc,
  maxDepth?: number
): Point[] | undefined => {
  if (maxDepth == null) {
    maxDepth = Infinity;
  }

  // validate start and goal
  if (hitFunc(start) || hitFunc(goal)) {
    return undefined;
  }

  // This is goal at first
  const currentCost = calcCost(start, goal);
  if (currentCost === 0) {
    return [];
  }

  const initialNode: Node = { pos: start, cost: currentCost, parent: null };

  let openedNodes: Node[] = [initialNode];
  let closedNodes: Node[] = [];

  const transferOpenToClose = (node: Node) => {
    openedNodes = openedNodes.filter(n => n !== node);
    closedNodes.push(node);
  };

  const reopenClosedNode = (node: Node) => {
    closedNodes = closedNodes.filter(n => n !== node);
    openedNodes.push({
      ...node,
      cost: node.cost
    });
  };

  let cnt = 0;
  while (++cnt <= maxDepth) {
    const node = selectMinCostNode(openedNodes);
    if (node == null) {
      return undefined;
    } else {
      // finished?
      if (node.cost === 0) {
        return resolvePathFromGoalToStart(node);
      }

      // transfer node from open to close
      transferOpenToClose(node);

      const neibors: Node[] = searchNeibors(node, goal, hitFunc);

      for (const neibor of neibors) {
        const openedNodeIndex: number = openedNodes.findIndex(n => {
          return neibor.pos.x === n.pos.x && neibor.pos.y === n.pos.y;
        });

        // Try to search openedNodes
        if (openedNodeIndex > -1) {
          // Rewrite opened node cost
          const openedNode: Node = openedNodes[openedNodeIndex];
          if (neibor.cost < openedNode.cost) {
            openedNodes[openedNodeIndex] = {
              ...openedNode,
              cost: neibor.cost
            };
          }
          continue;
        }

        // Try to search closedNodes
        const closedNodeIndex: number = closedNodes.findIndex(c => {
          return neibor.pos.x === c.pos.x && neibor.pos.y === c.pos.y;
        });
        if (closedNodeIndex > -1) {
          // Re-open closed nodes if cost updated
          const closedNode: Node = closedNodes[closedNodeIndex];
          if (closedNode.cost > neibor.cost) {
            reopenClosedNode(closedNode);
          }
          continue;
        }

        // Add openedNodes
        openedNodes.push(neibor);
      }
    }
  }
};