Skip to content

Commit

Permalink
pathfinding performance improvements
Browse files Browse the repository at this point in the history 
Signed-off-by: Tim Deubler <tim.deubler@here.com>
  • Loading branch information
@TerminalTim
TerminalTim committed Jan 30, 2024
1 parent 8871418 commit eb8bd43
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 40 deletions.
59 changes: 36 additions & 23 deletions packages/common/src/AStar.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,17 +33,21 @@ class Node implements AStarNode {
f: number;
parent: Node | null;

id: number;

constructor(point: number[], g: number, h: number, parent: Node | null = null) {
this.point = point;
this.g = g;
this.h = h;
this.f = g + h;
this.parent = parent;
this.id = AStar.pointKey(this);
}
}

export class AStar {
static precision = 1e5;

static calculateDistance(point1: number[], point2: number[]): number {
// const dx = point2[0] - point1[0];
// const dy = point2[1] - point1[1];
Expand All @@ -61,64 +65,73 @@ export class AStar {
// return point1[0] === point2[0] && point1[1] === point2[1];
}

private static pointKey(node: AStarNode): number {
static pointKey(node: AStarNode): number {
const precision = AStar.precision;
const point = node.point;
return (point[0] * precision ^ 0) * 1e7 + (point[1] * precision ^ 0);
}

private static reconstructPath(current: Node) {
const path: AStarNode[] = [];
while (current !== null) {
path.unshift(current);
current = current.parent;
}
return path;
}

public static findPath(
from: AStarNode,
endNode: AStarNode,
getNeighbors: (node: AStarNode) => AStarNode[],
weight: (nodeA: AStarNode, nodeB: AStarNode) => number = AStar.weight
): AStarNode[] | null {
const start = from.point;
const endCoordinate = endNode.point;
const openList = new BinaryHeap<Node>((a, b) => a.f - b.f);
const closedList = new Set<number>();
// const startNode = new NavNode(start, 0, AStar.calculateDistance(start, endCoordinate));
const startNode = new Node(start, 0, Infinity);
const closedList = new Map<string | number, boolean>();
// const startNode = new NavNode(start, 0, AStar.calculateDistance(start, endNode.point));
const startNode = new Node(from.point, 0, Infinity);

startNode.data = from.data;

openList.push(startNode);

const endNodeId = this.pointKey(endNode);

while (openList.size() > 0) {
const currentNode = openList.pop()!;

if (AStar.isPointEqual(currentNode.point, endCoordinate)) {
// reconstruct the path
const path: AStarNode[] = [];
let current: Node | null = currentNode;
while (current !== null) {
path.unshift(current);
current = current.parent;
}
return path;
if (currentNode.id === endNodeId) {
// if (AStar.isPointEqual(currentNode.point, endNode.point)) {
return this.reconstructPath(currentNode);
}

const pointKey = AStar.pointKey(currentNode);
closedList.add(pointKey);
closedList.set(currentNode.id, true);

for (const neighborNode of getNeighbors(currentNode)) {
const {point: neighbor, data} = neighborNode;
const neighborKey = AStar.pointKey(neighborNode);
const {point, data} = neighborNode;
const neighborKey = this.pointKey(neighborNode);

if (closedList.has(neighborKey)) {
continue;
}

const g = currentNode.g + weight(currentNode, neighborNode);
const h = weight(neighborNode, endNode);
// const h = AStar.calculateDistance(neighborNode.point, endNode.point);
const existingNode = openList.find((node) => AStar.isPointEqual(node.point, neighbor));
if (existingNode) {
// const existingNodeIndex = openList.findIndex((node) => AStar.isPointEqual(node.point, point));
const existingNodeIndex = openList.findIndex((node) => node.id == neighborKey);

if (existingNodeIndex != -1) {
const existingNode = openList.get(existingNodeIndex);
if (g < existingNode.g) {
existingNode.g = g;
existingNode.h = h;
existingNode.f = g + h;
existingNode.parent = currentNode;
existingNode.data = data;
openList.adjustElement(existingNodeIndex);
}
} else {
const newNode = new Node(neighbor, g, h, currentNode);
const newNode = new Node(point, g, h, currentNode);
newNode.data = data;
openList.push(newNode);
}
Expand Down
48 changes: 41 additions & 7 deletions packages/common/src/BinaryHeap.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,18 @@ export class BinaryHeap<T> {
this.compare = compare;
}

find(predicate: (this: void, value: T, index: number, obj: T[]) => boolean) {
get(index: number) {
return this.heap[index];
}

find(predicate: (value: T, index: number, obj: T[]) => boolean) {
return this.heap.find(predicate);
}

findIndex(predicate: (this: void, value: T, index: number, obj: T[]) => boolean): number {
return this.heap.findIndex(predicate);
}

includes(value: T) {
return this.heap.includes(value);
}
Expand All @@ -52,6 +60,29 @@ export class BinaryHeap<T> {
return result;
}

adjustElement(index: number): void {
const element = this.heap[index];
const parentIndex = Math.floor((index - 1) / 2);
const parent = this.heap[parentIndex];

if (parentIndex > 0 && this.compare(element, parent) < 0) {
this.bubbleUp(index);
} else {
this.sinkDown(index);
}
}

remove(value: T): void {
const index = this.heap.findIndex((item) => item === value);
if (index !== -1) {
const end = this.heap.pop()!;
if (index !== this.heap.length) {
this.heap[index] = end;
this.adjustElement(index);
}
}
}

size(): number {
return this.heap.length;
}
Expand All @@ -71,30 +102,33 @@ export class BinaryHeap<T> {
private sinkDown(index: number): void {
const length = this.heap.length;
const element = this.heap[index];

while (true) {
let leftChildIndex = 2 * index + 1;
let rightChildIndex = 2 * index + 2;
let swap = null;
let leftChild;
let rightChild;

if (leftChildIndex < length) {
leftChild = this.heap[leftChildIndex];
const leftChild = this.heap[leftChildIndex];
if (this.compare(leftChild, element) < 0) {
swap = leftChildIndex;
}
}
if (rightChildIndex < length) {
rightChild = this.heap[rightChildIndex];
if ((swap === null && this.compare(rightChild, element) < 0) ||
(swap !== null && this.compare(rightChild, leftChild!) < 0)) {
const rightChild = this.heap[rightChildIndex];
if (
(swap === null && this.compare(rightChild, element) < 0) ||
(swap !== null && this.compare(rightChild, this.heap[swap]!) < 0)
) {
swap = rightChildIndex;
}
}

if (swap === null) break;
this.heap[index] = this.heap[swap];
this.heap[swap] = element;
index = swap;
}
}
}

12 changes: 6 additions & 6 deletions packages/core/src/providers/FeatureProvider.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -736,28 +736,28 @@ export class FeatureProvider extends Provider {
/**
* Object representing the source road segment of the turn.
*/
from: {
readonly from: {
/**
* GeoJSON Feature representing the source road segment.
*/
link: Feature<'LineString'>,
readonly link: Feature<'LineString'>,
/**
* Index of the Coordinates array of the source road segment.
*/
index: number
readonly index: number
},
/**
* Object representing the destination road segment of the turn.
*/
to: {
readonly to: {
/**
* GeoJSON Feature representing the destination road segment.
*/
link: Feature<'LineString'>,
readonly link: Feature<'LineString'>,
/**
* Index of the Coordinates array of the destination road segment.
*/
index: number
readonly index: number
}
}) => boolean,
/**
Expand Down
10 changes: 6 additions & 4 deletions packages/core/src/route/Route.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
* License-Filename: LICENSE
*/
import {AStar, AStarNode} from '@here/xyz-maps-common';
import {Feature, FeatureProvider, GeoJSONCoordinate} from '@here/xyz-maps-core';
import {Feature, FeatureProvider} from '@here/xyz-maps-core';

type NodeData = { link: Feature, index: number };

Expand All @@ -29,12 +29,14 @@ type Weight = (options: { feature: Feature; distance: number; from: number[]; to
// return [0, feature.geometry.coordinates.length - 1];
// };

type Turn = { from: NodeData, to: NodeData };

export class PathFinder {
static async findPath(
provider: FeatureProvider,
fromNode: Node,
toNode: Node,
isTurnAllowed: (turn: { from: NodeData, to: NodeData }) => boolean,
isTurnAllowed: (turn: Turn) => boolean,
weight?: Weight
// filterCoordinates: ((feature: Feature) => number[]) | undefined = defaultCoordinatesFilter
): Promise<Node[]> {
Expand All @@ -43,8 +45,8 @@ export class PathFinder {
const getNeighbor = (node: Node): Node[] => {
const point = node.point;
const fromLink = node.data.link;
const turn = {from: node.data, to: {link: null, index: null}};
const neighbors: { point: number[], data: { link: Feature, index: number } }[] = [];
const turn = {from: node.data, to: null};
const neighbors: Node[] = [];
const geoJSONFeatures = <Feature[]>provider.search({point: {longitude: point[0], latitude: point[1]}, radius: .5});
for (const feature of geoJSONFeatures) {
if (feature.geometry.type != 'LineString' /* || feature.id == fromLink.id*/) continue;
Expand Down

0 comments on commit eb8bd43

Please sign in to comment.