nanomap/tools/ShortestPath.js

246 lines
7.1 KiB
JavaScript
Raw Permalink Normal View History

2023-12-27 11:06:29 +08:00
import {MinPriorityQueue} from "@datastructures-js/priority-queue";
import {sill, noexcept, sill_unwrap} from "./Debug";
import {find_common} from "./Misc";
2023-12-27 11:06:29 +08:00
function __haversine_distance(p1, p2) {
const toRadians = (degrees) => {
return degrees * Math.PI / 180;
};
const [lat1, lon1] = p1;
const [lat2, lon2] = p2;
const R = 6371;
const dLat = toRadians(lat2 - lat1);
const dLon = toRadians(lon2 - lon1);
const a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
Math.cos(toRadians(lat1)) * Math.cos(toRadians(lat2)) *
Math.sin(dLon / 2) * Math.sin(dLon / 2);
const c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
if (R * c < 0) console.warn("WARNING!!!");
return R * c;
}
/**
* Use Dijkstra algorithm to find the shortest path.
* @param nodes node index list
* @param ways unused in this implementation
* @param loc unused in this implementation
2023-12-27 11:06:29 +08:00
* @param ch adjacent table
* @param count unused in this implementation
* @param aff unused in this implementation
2023-12-27 11:06:29 +08:00
* @param u start node id
* @param p destination node id
*/
function __dijkstra(nodes, ways, loc, ch, count, aff, u, p) {
// sill(`node count: ${Object.keys(nodes).length}`);
2023-12-27 11:06:29 +08:00
const dis = {};
const fa = {};
const vis = new Set();
const pq = new MinPriorityQueue();
dis[u] = 0;
pq.push([0, u]);
while (!pq.isEmpty()) {
const [d, v] = pq.pop();
if (vis.has(v) || !ch[v]) continue;
if (v === p) {
break;
}
2023-12-27 11:06:29 +08:00
vis.add(v);
const t = ch[v].length;
for (let j = 0; j < t; ++j) {
const [c, w] = ch[v][j];
2023-12-27 11:06:29 +08:00
if (!nodes[c]) continue;
// const w = get_weight(v, c);
2023-12-27 11:06:29 +08:00
if (!dis[c] || d + w < dis[c]) {
dis[c] = d + w;
pq.push([dis[c], c]);
fa[c] = v;
}
}
}
let curr = p;
const res = [p];
vis.clear();
while (fa[curr]) {
curr = fa[curr].toString();
if (vis.has(curr)) {
// sill(`Cycle at ${curr}`);
2023-12-27 11:06:29 +08:00
break;
}
vis.add(curr);
res.push(curr);
}
// sill("finished Dijkstra.");
return res;
}
/**
* Use Dijkstra algorithm with Obvious optimization to find the shortest path.
* @param nodes node index list
* @param ways node list for each way
* @param loc relative location in the way
* @param ch adjacent table
* @param count determine isolated nodes
* @param aff affiliation of isolated nodes
* @param u start node id
* @param p destination node id
*/
function __obvious_dijkstra(nodes, ways, loc, ch, count, aff, u, p) {
// sill(`node count: ${Object.keys(nodes).length}`);
const dis = {};
const fa = {};
const vis = new Map();
const pq = new MinPriorityQueue();
dis[u] = 0;
pq.push([0, u]);
while (!pq.isEmpty()) {
const [d, v] = pq.pop();
if (vis.has(v) || !ch[v]) {
// sill(!ch[v]);
continue;
}
if (v === p) {
// sill('break');
break;
}
// sill('loop');
vis.set(v,true);
const t = ch[v].length;
for (let j = 0; j < t; ++j) {
const [c, w] = ch[v][j];
if (!nodes[c]) continue;
// sill(w);
if ((!dis[c] || d + w < dis[c])) {
dis[c] = d + w;
pq.push([dis[c], c]);
fa[c] = v;
}
}
}
// sill_unwrap(fa);
let curr = p;
const res = [p];
vis.clear();
while (fa[curr]) {
const prev = curr;
curr = fa[curr];
if (vis.has(curr)) {
break;
}
vis.set(curr,true);
// res.push(curr);
const way_id = find_common(aff[prev], aff[curr]);
const way = ways[way_id];
if(!way) {
continue;
}
const p_oc = loc[prev][way_id];
const c_oc = loc[curr][way_id];
if (p_oc < c_oc) {
for (let _p = p_oc + 1; _p <= c_oc; ++_p) {
res.push(way[_p]);
}
} else {
for (let _p = p_oc - 1; _p >= c_oc; --_p) {
res.push(way[_p]);
}
}
}
// sill("finished Obvious Dijkstra.");
return res;
}
/**
* Use A-star algorithm with Obvious optimization to find the shortest path.
* @param nodes node index list
* @param ways node list for each way
* @param loc relative location in the way
* @param ch adjacent table
* @param count determine isolated nodes
* @param aff affiliation of isolated nodes
* @param u start node id
* @param p destination node id
* @param adaptive if the coefficient is hard-coded
*/
function __obvious_a_star(nodes, ways, loc, ch, count, aff, u, p, adaptive = false) {
// sill(`node count: ${Object.keys(nodes).length}`);
const default_coefficient = 1.1;
const linear = (y) => y;
const curve = (y) => 0.8 * y * y;
const inversed_sigmoid = (y) => Math.log(y/(1.0-y));
const heuristic = (current_distance, node_id) => {
const estimate = haversine_distance(nodes[node_id], nodes[p]);
const coef = (!adaptive) ? default_coefficient : (
default_coefficient * curve(current_distance / (current_distance + estimate))
);
return coef * estimate;
};
const dis = {};
const fa = {};
const vis = new Map();
const pq = new MinPriorityQueue();
dis[u] = 0;
pq.push([0, 0, u]);
while (!pq.isEmpty()) {
const [_, d, v] = pq.pop();
if (vis.has(v) || !ch[v]) {
// sill(!ch[v]);
continue;
}
if (v === p) {
// sill('break');
break;
}
// sill('loop');
vis.set(v,true);
const t = ch[v].length;
for (let j = 0; j < t; ++j) {
const [c, w] = ch[v][j];
if (!nodes[c]) continue;
// sill(w);
if ((!dis[c] || d + w < dis[c])) {
dis[c] = d + w;
pq.push([dis[c] + heuristic(dis[c], c), dis[c], c]);
fa[c] = v;
}
}
}
// sill_unwrap(fa);
let curr = p;
const res = [p];
vis.clear();
while (fa[curr]) {
const prev = curr;
curr = fa[curr];
if (vis.has(curr)) {
break;
}
vis.set(curr,true);
// res.push(curr);
const way_id = find_common(aff[prev], aff[curr]);
const way = ways[way_id];
if(!way) {
continue;
}
const p_oc = loc[prev][way_id];
const c_oc = loc[curr][way_id];
if (p_oc < c_oc) {
for (let _p = p_oc + 1; _p <= c_oc; ++_p) {
res.push(way[_p]);
}
} else {
for (let _p = p_oc - 1; _p >= c_oc; --_p) {
res.push(way[_p]);
}
}
}
// sill("finished Obvious Dijkstra.");
2023-12-27 11:06:29 +08:00
return res;
}
export const haversine_distance = noexcept(__haversine_distance);
export const dijkstra = noexcept(__dijkstra);
export const obvious_dijkstra = noexcept(__obvious_dijkstra);
export const obvious_a_star = noexcept(__obvious_a_star);