From 73929ae011ec4af91485f8b7dc3b201b7c7d4c8a Mon Sep 17 00:00:00 2001 From: Tim Deubler Date: Thu, 18 Jan 2024 18:41:29 +0100 Subject: [PATCH] added(core): Introducing the ["findPath"](https://heremaps.github.io/xyz-maps/docs/classes/core.featureprovider.html#findpath) method for optimal client-side path finding on a GeoJSON road network supporting advanced options such as custom turn restrictions and weights. Signed-off-by: Tim Deubler --- packages/common/src/AStar.ts | 130 ++++++++++ packages/common/src/BinaryHeap.ts | 100 ++++++++ packages/common/src/index.ts | 6 +- packages/core/src/features/Feature.ts | 8 +- packages/core/src/features/GeoJSON.ts | 8 +- .../core/src/providers/FeatureProvider.ts | 232 ++++++++++++++++++ packages/core/src/route/Route.ts | 84 +++++++ 7 files changed, 559 insertions(+), 9 deletions(-) create mode 100644 packages/common/src/AStar.ts create mode 100644 packages/common/src/BinaryHeap.ts create mode 100644 packages/core/src/route/Route.ts diff --git a/packages/common/src/AStar.ts b/packages/common/src/AStar.ts new file mode 100644 index 000000000..c2707d9cf --- /dev/null +++ b/packages/common/src/AStar.ts @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2019-2023 HERE Europe B.V. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * SPDX-License-Identifier: Apache-2.0 + * License-Filename: LICENSE + */ + +import {BinaryHeap} from './BinaryHeap'; +import {distance} from './geotools'; + +export type AStarNode = { point: number[], data?: any }; + +class Node implements AStarNode { + point: number[]; + data?: any; + // "real" cost to reach the node (start->node) + g: number; + // approximate cost to reach the goal node (node->goal) + h: number; + // total cost (g+h) + f: number; + parent: Node | null; + + 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; + } +} + +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]; + // return Math.sqrt(dx * dx + dy * dy); + return distance(point1, point2); + } + + private static weight(nodeA: AStarNode, nodeB: AStarNode): number { + return AStar.calculateDistance(nodeA.point, nodeB.point); + } + + static isPointEqual(point1: number[], point2: number[]): boolean { + const precision = AStar.precision; + return (point1[0] * precision ^ 0) === (point2[0] * precision ^ 0) && (point1[1] * precision ^ 0) === (point2[1] * precision ^ 0); + // return point1[0] === point2[0] && point1[1] === point2[1]; + } + + private static pointKey(node: AStarNode): number { + const precision = AStar.precision; + const point = node.point; + return (point[0] * precision ^ 0) * 1e7 + (point[1] * precision ^ 0); + } + + 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((a, b) => a.f - b.f); + const closedList = new Set(); + // const startNode = new NavNode(start, 0, AStar.calculateDistance(start, endCoordinate)); + const startNode = new Node(start, 0, Infinity); + startNode.data = from.data; + + openList.push(startNode); + + 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; + } + + const pointKey = AStar.pointKey(currentNode); + closedList.add(pointKey); + for (const neighborNode of getNeighbors(currentNode)) { + const {point: neighbor, data} = neighborNode; + const neighborKey = AStar.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) { + if (g < existingNode.g) { + existingNode.g = g; + existingNode.h = h; + existingNode.f = g + h; + existingNode.parent = currentNode; + existingNode.data = data; + } + } else { + const newNode = new Node(neighbor, g, h, currentNode); + newNode.data = data; + openList.push(newNode); + } + } + } + // no path found + return null; + } +} diff --git a/packages/common/src/BinaryHeap.ts b/packages/common/src/BinaryHeap.ts new file mode 100644 index 000000000..713b2983b --- /dev/null +++ b/packages/common/src/BinaryHeap.ts @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2019-2023 HERE Europe B.V. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * SPDX-License-Identifier: Apache-2.0 + * License-Filename: LICENSE + */ +export class BinaryHeap { + private heap: T[]; + private compare: (a: T, b: T) => number; + + constructor(compare: (a: T, b: T) => number) { + this.heap = []; + this.compare = compare; + } + + find(predicate: (this: void, value: T, index: number, obj: T[]) => boolean) { + return this.heap.find(predicate); + } + + includes(value: T) { + return this.heap.includes(value); + } + + push(value: T): void { + this.heap.push(value); + this.bubbleUp(this.heap.length - 1); + } + + pop(): T | undefined { + const {heap} = this; + + if (!heap.length) return; + + const result = heap[0]; + const end = heap.pop()!; + if (heap.length) { + heap[0] = end; + this.sinkDown(0); + } + return result; + } + + size(): number { + return this.heap.length; + } + + private bubbleUp(index: number): void { + const element = this.heap[index]; + while (index > 0) { + const parentIndex = Math.floor((index - 1) / 2); + const parent = this.heap[parentIndex]; + if (this.compare(element, parent) >= 0) break; + this.heap[parentIndex] = element; + this.heap[index] = parent; + index = parentIndex; + } + } + + 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]; + 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)) { + swap = rightChildIndex; + } + } + if (swap === null) break; + this.heap[index] = this.heap[swap]; + this.heap[swap] = element; + index = swap; + } + } +} diff --git a/packages/common/src/index.ts b/packages/common/src/index.ts index 40dfca494..31d7dace9 100644 --- a/packages/common/src/index.ts +++ b/packages/common/src/index.ts @@ -30,6 +30,8 @@ import Set from './Set'; import Map from './Map'; import Queue from './Queue'; import * as vec3 from './Vec3'; +import {AStar, AStarNode} from './AStar'; +import {BinaryHeap} from './BinaryHeap'; // make sure global ns is also available for webpack users. let scp: any = global; @@ -38,10 +40,10 @@ let scp: any = global; // support for deprecated root namespace (global).HERE = (global).here; -const common = {LRU, TaskManager, Listener, parseJSONArray, JSUtils, geotools, global, Queue, Set, Map, vec3, geometry}; +const common = {AStar, BinaryHeap, LRU, TaskManager, Listener, parseJSONArray, JSUtils, geotools, global, Queue, Set, Map, vec3, geometry}; scp.common = common; -export {LRU, TaskManager, Task, TaskOptions, Listener, parseJSONArray, JSUtils, geotools, global, Queue, Set, Map, vec3, geometry}; +export {AStar, AStarNode, BinaryHeap, LRU, TaskManager, Task, TaskOptions, Listener, parseJSONArray, JSUtils, geotools, global, Queue, Set, Map, vec3, geometry}; export default common; diff --git a/packages/core/src/features/Feature.ts b/packages/core/src/features/Feature.ts index 359cd2c9e..eaee4ffeb 100644 --- a/packages/core/src/features/Feature.ts +++ b/packages/core/src/features/Feature.ts @@ -24,7 +24,7 @@ import {GeoJSONFeature, GeoJSONBBox} from './GeoJSON'; /** * represents a Feature in GeoJSON Feature format. */ -export class Feature implements GeoJSONFeature { +export class Feature implements GeoJSONFeature { /** * id of the feature. */ @@ -33,7 +33,9 @@ export class Feature implements GeoJSONFeature { /** * The properties associated with the feature. */ - properties: { [name: string]: any; } | null; + properties: { + [name: string]: any; + } | null; /** * The type of the feature is a string with 'Feature' as its value. @@ -83,7 +85,7 @@ export class Feature implements GeoJSONFeature { *``` */ geometry: { - type: 'Point' | 'MultiPoint' | 'LineString' | 'MultiLineString' | 'Polygon' | 'MultiPolygon' | string, + type: 'Point' | 'MultiPoint' | 'LineString' | 'MultiLineString' | 'Polygon' | 'MultiPolygon' | GeometryType | string, coordinates: any[] /** * cached polygon centroid diff --git a/packages/core/src/features/GeoJSON.ts b/packages/core/src/features/GeoJSON.ts index 61d6db8c7..b73d84682 100644 --- a/packages/core/src/features/GeoJSON.ts +++ b/packages/core/src/features/GeoJSON.ts @@ -40,14 +40,14 @@ export type GeoJSONCoordinate = number[]; // [number, number] | [number, number, /** * A GeoJSON Feature object. */ -export interface GeoJSONFeature { +export interface GeoJSONFeature { /** * id of the feature. */ id?: string | number; /** - * Type of a GeoJSONFeature is 'Feature' + * Type of GeoJSONFeature is 'Feature' */ type: 'Feature' | string; @@ -105,7 +105,7 @@ export interface GeoJSONFeature { *``` */ geometry: { - type: 'Point' | 'MultiPoint' | 'LineString' | 'MultiLineString' | 'Polygon' | 'MultiPolygon' | string, + type: 'Point' | 'MultiPoint' | 'LineString' | 'MultiLineString' | 'Polygon' | 'MultiPolygon' | GeometryType | string, coordinates: GeoJSONCoordinate | GeoJSONCoordinate[] | GeoJSONCoordinate[][] | GeoJSONCoordinate[][][] }; } @@ -121,5 +121,5 @@ export interface GeoJSONFeatureCollection { /** * An array of {@link GeoJSONFeature | GeoJSONFeatures}. */ - features: GeoJSONFeature[] + features: GeoJSONFeature[]; } diff --git a/packages/core/src/providers/FeatureProvider.ts b/packages/core/src/providers/FeatureProvider.ts index 430d3370a..5cb4d7c73 100644 --- a/packages/core/src/providers/FeatureProvider.ts +++ b/packages/core/src/providers/FeatureProvider.ts @@ -32,6 +32,7 @@ import { import {TileProviderOptions} from './TileProvider/TileProviderOptions'; import {GeoPoint} from '../geo/GeoPoint'; import {GeoRect} from '../geo/GeoRect'; +import {PathFinder} from '../route/Route'; const REMOVE_FEATURE_EVENT = 'featureRemove'; @@ -677,6 +678,237 @@ export class FeatureProvider extends Provider { return feature; }; + + /** + * Finds the optimal path between two coordinates on a GeoJSON road network, considering various options. + * By default, the weight function returns the distance of the road segment, a lower distance implies a shorter route and is considered more favorable. + * If you have specific criteria such as road quality, traffic conditions, or other factors influencing the desirability of a road segment, you can customize the weight function accordingly. + * Only data available locally will be taken into account for pathfinding. + * + * @experimental + * + * @param options - The options object containing parameters for finding the path. + * @returns {Promise<{ + * features: Feature[]; + * readonly path: GeoJSONFeature; + * readonly distance: number; + * from: GeoJSONCoordinate; + * to: GeoJSONCoordinate; + * }>} A Promise that resolves to an object containing the path, additional information, and the distance of the path. + * + * @example + * ```typescript + * const pathOptions = { + * from: [startLongitude, startLatitude], + * to: [endLongitude, endLatitude], + * // optional + * allowTurn: (turn) => { + * // Custom logic to determine whether a turn is allowed + * return true; + * }, + * // optional + * weight: (data) => { + * // Custom weight function to determine the cost of traversing a road segment + * return data.distance; // Default implementation uses distance as the weight + * }, + * }; + * const result = await findPath(pathOptions); + * console.log(result); + * ``` + */ + async findPath(options: { + /** + * The starting coordinates defining the path. + */ + from: GeoJSONCoordinate | GeoPoint, + /** + * The ending coordinates of the path. + */ + to: GeoJSONCoordinate | GeoPoint, + /** + * Optional callback function to determine if a turn is allowed between road segments. + * If not provided, all turns are considered allowed. + * + * @param {object} turn - Object containing information about the turn, including source and destination road segments. + * @returns {boolean} `true` if the turn is allowed, `false` otherwise. + */ + allowTurn?: (turn: { + /** + * Object representing the source road segment of the turn. + */ + from: { + /** + * GeoJSON Feature representing the source road segment. + */ + link: Feature<'LineString'>, + /** + * Index of the Coordinates array of the source road segment. + */ + index: number + }, + /** + * Object representing the destination road segment of the turn. + */ + to: { + /** + * GeoJSON Feature representing the destination road segment. + */ + link: Feature<'LineString'>, + /** + * Index of the Coordinates array of the destination road segment. + */ + index: number + } + }) => boolean, + /** + * Optional callback function to determine the weight (cost) of traversing a road segment. + * + * @param {object} data - Object containing information about the road segment. + * @returns {number} A numerical value representing the weight or cost of traversing the road segment. Default is the distance in meter. + */ + weight?: (data: { + /** + * Starting coordinates of the road segment. + */ + from: GeoJSONCoordinate, + /** + * Ending coordinates of the road segment. + */ + to: GeoJSONCoordinate, + /** + * Feature representing the road segment. + */ + feature: Feature<'LineString'>, + /** + * Direction of traversal on the road segment. + */ + direction: 'START_TO_END' | 'END_TO_START', + /** + * The Distance of the road in meters. + */ + distance: number + }) => number + }) + /** + * A Promise that resolves to an object containing the path and additional information. + */ + : Promise<{ + /** + * A GeoJSON Feature of geometry type MultiLineString representing the geometry of the found path. + */ + readonly path: GeoJSONFeature<'MultiLineString'>; + /** + * An array of GeoJSON features representing the road segments in the path. + */ + features: Feature<'LineString'>[]; + /** + * the distance in meter of the path. + */ + readonly distance: number; + /** + * The starting coordinates of the path. + */ + from: GeoJSONCoordinate; + /** + * The end coordinates of the path. + */ + to: GeoJSONCoordinate + }> { + if (!options?.from || !options?.to) return null; + + let {from, to} = options; + + if (typeof (from as GeoPoint).longitude == 'number') { + from = [(from as GeoPoint).longitude, (from as GeoPoint).latitude]; + } + if (typeof (to as GeoPoint).longitude == 'number') { + to = [(to as GeoPoint).longitude, (to as GeoPoint).latitude]; + } + + function findNearestCoordinate(searchPoint: number[], lineCoordinates: number[][]): { point: number[], distance: number } { + let nearestPoint = null; + let minDistance = Infinity; + for (let i = 0; i < lineCoordinates.length; i++) { + const currentPoint = lineCoordinates[i]; + const currentDistance = geotools.distance(searchPoint, currentPoint); + if (currentDistance < minDistance) { + minDistance = currentDistance; + nearestPoint = currentPoint; + } + } + return {point: nearestPoint, distance: minDistance}; + } + + const findNearestNode = (point: number[], radius: number = 10, maxRadius = 10_000) => { + const features = this.search({point: {longitude: point[0], latitude: point[1]}, radius}); + if (!features.length && radius < maxRadius) return findNearestNode(point, radius * 10); + + const start = {distance: Infinity, feature: null, point: null, index: null}; + for (let feature of features) { + if (feature.geometry.type != 'LineString') continue; + const {coordinates} = feature.geometry; + const last = coordinates.length - 1; + const result = findNearestCoordinate(point, [coordinates[0], coordinates[last]]); + if (result.distance < start.distance) { + start.distance = result.distance; + start.point = result.point; + start.feature = feature; + start.index = start.point === coordinates[0] ? 0 : last; + } + } + return start; + }; + + const start = findNearestNode(from); + const end = findNearestNode(to); + + if (!start.feature || !end.feature) return null; + + const fromNode = { + point: start.point, + data: { + link: start.feature, + index: start.index + } + }; + const toNode = { + point: end.point, + data: { + link: end.feature, + index: end.index + } + }; + + const nodes = await PathFinder.findPath(this, fromNode, toNode, options?.allowTurn || (() => true), options?.weight); + + return nodes?.length && { + from: [...fromNode.point], + to: [...toNode.point], + features: nodes.map((node) => node.data.link) as Feature<'LineString'>[], + get distance() { + let distance = 0; + for (let lineString of this.path.geometry.coordinates) { + for (let i = 1, len = lineString.length; i < len; i++) { + distance += geotools.distance(lineString[i - 1], lineString[i]); + } + // distance += geotools.distance(lineString[0], lineString[lineString.length - 1]); + } + return distance; + }, + get path() { + return { + type: 'Feature', + geometry: { + type: 'MultiLineString', + coordinates: this.features.map((feature) => feature.geometry.coordinates) + }, + properties: {} + }; + } + }; + } + + /** * Clear all tiles and features of a given bounding box or do a full wipe if no parameter is given. * diff --git a/packages/core/src/route/Route.ts b/packages/core/src/route/Route.ts new file mode 100644 index 000000000..5e075f79c --- /dev/null +++ b/packages/core/src/route/Route.ts @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2019-2023 HERE Europe B.V. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * SPDX-License-Identifier: Apache-2.0 + * License-Filename: LICENSE + */ +import {AStar, AStarNode} from '@here/xyz-maps-common'; +import {Feature, FeatureProvider, GeoJSONCoordinate} from '@here/xyz-maps-core'; + +type NodeData = { link: Feature, index: number }; + +type Node = AStarNode & { data: NodeData }; + +type Weight = (options: { feature: Feature; distance: number; from: number[]; to: number[]; direction: 'START_TO_END' | 'END_TO_START' }) => number; + +// const defaultCoordinatesFilter = (feature: Feature<'LineString'>) => { +// return [0, feature.geometry.coordinates.length - 1]; +// }; + +export class PathFinder { + static async findPath( + provider: FeatureProvider, + fromNode: Node, + toNode: Node, + isTurnAllowed: (turn: { from: NodeData, to: NodeData }) => boolean, + weight?: Weight + // filterCoordinates: ((feature: Feature) => number[]) | undefined = defaultCoordinatesFilter + ): Promise { + return new Promise((resolve, reject) => { + // console.time('route'); + 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 geoJSONFeatures = 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; + const coordinates = feature.geometry.coordinates; + const firstCoordinateIndex = 0; + const lastCoordinateIndex = coordinates.length - 1; + const index = AStar.isPointEqual(point, coordinates[firstCoordinateIndex]) + ? lastCoordinateIndex + : AStar.isPointEqual(point, coordinates[lastCoordinateIndex]) + ? firstCoordinateIndex + : null; + + if (index != null) { + const data = turn.to = {link: feature, index}; + + if (fromLink == feature || isTurnAllowed(turn)) { + neighbors.push({point: coordinates[index], data}); + } + } + } + return neighbors; + }; + + const path = AStar.findPath(fromNode, toNode, getNeighbor, weight && ((a, b) => { + return weight({ + from: a.point, + to: b.point, + direction: b.data.index ? 'START_TO_END' : 'END_TO_START', + feature: b.data.link, + distance: AStar.calculateDistance(a.point, b.point) + }); + })) as Node[]; + // console.timeEnd('route'); + resolve(path); + }); + } +};