1003 lines
33 KiB
C++
1003 lines
33 KiB
C++
#ifdef __EMSCRIPTEN__
|
|
#include <emscripten/bind.h>
|
|
#include <emscripten/val.h>
|
|
#endif
|
|
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <arpa/inet.h>
|
|
#include <vector>
|
|
#include <map>
|
|
#include <set>
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
|
|
// Constants
|
|
const float GRAVITY_ACCELERATION = 9.81;
|
|
|
|
|
|
// Data structures
|
|
struct Connection {
|
|
int connectedPointNumber;
|
|
float distance;
|
|
float course;
|
|
uint8_t speedLimit;
|
|
bool motorway;
|
|
bool tunnel;
|
|
bool againstOneWay;
|
|
};
|
|
|
|
struct RoadNode{
|
|
float positionX;
|
|
float positionY;
|
|
float positionZ;
|
|
|
|
Connection connectionOne;
|
|
Connection connectionTwo;
|
|
std::vector<Connection> *extraConnections;
|
|
};
|
|
|
|
struct RoadNodeSet {
|
|
uint32_t numberOfNodes;
|
|
RoadNode* roadNodes;
|
|
};
|
|
|
|
struct SearchNodeInfo {
|
|
float distanceFromPrevious;
|
|
float currentSpeed;
|
|
};
|
|
|
|
struct SearchResult {
|
|
uint32_t startingNode;
|
|
std::map<uint32_t, uint32_t> previous;
|
|
std::map<uint32_t, SearchNodeInfo> reachableNodes;
|
|
bool reverse;
|
|
};
|
|
|
|
struct JSNodeInfo {
|
|
uint32_t nodeId;
|
|
float positionX;
|
|
float positionY;
|
|
float positionZ;
|
|
float distanceFromStart;
|
|
float currentSpeed;
|
|
float requiredSpeed;
|
|
};
|
|
|
|
struct JSSearchResult {
|
|
std::vector<JSNodeInfo> endPoints;
|
|
bool reverse;
|
|
};
|
|
|
|
struct ListNode {
|
|
int id;
|
|
float currentSpeed;
|
|
float currentCourse;
|
|
|
|
ListNode* next = NULL;
|
|
};
|
|
|
|
struct PolygonCoordinate {
|
|
float x;
|
|
float y;
|
|
};
|
|
|
|
struct AreaSearchEntry {
|
|
uint32_t nodeId;
|
|
float positionX;
|
|
float positionY;
|
|
float longestRoute;
|
|
};
|
|
|
|
struct AreaSearchResult {
|
|
uint32_t remainingNodes;
|
|
std::vector<AreaSearchEntry> searchedNodes;
|
|
};
|
|
|
|
struct CurrentAreaSearch {
|
|
float minimumSpeed;
|
|
float maximumSpeed;
|
|
float maximumSpeedLimit;
|
|
float dragCoefficient;
|
|
bool allowMotorways;
|
|
bool allowTunnels;
|
|
bool allowAgainstOneway;
|
|
bool limitCornerSpeed;
|
|
|
|
std::vector<uint32_t> startNodes;
|
|
size_t startNodeCounter = 0;
|
|
std::vector<AreaSearchEntry> currentAreaSearchResults;
|
|
std::map<uint32_t, std::vector<uint32_t>> utilizedNodes;
|
|
};
|
|
|
|
// Data
|
|
RoadNodeSet set;
|
|
SearchResult lastSearchResult;
|
|
std::set<uint32_t> excludedNodes;
|
|
|
|
CurrentAreaSearch currentAreaSearch;
|
|
|
|
// Data loading functions
|
|
float getFloatFromBuffer(char* buffer) {
|
|
// First, cast the buffer to an int, in order to reverse bytes from network order to host order
|
|
uint32_t correctByteOrder = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
|
return *((float*) &correctByteOrder);
|
|
}
|
|
|
|
void addLinkToRoadNode(RoadNode* roadNodes, uint32_t nodeFrom, uint32_t nodeTo, uint8_t speedLimit, bool motorway, bool tunnel, bool againstOneWay) {
|
|
float fromX = roadNodes[nodeFrom].positionX;
|
|
float fromY = roadNodes[nodeFrom].positionY;
|
|
float toX = roadNodes[nodeTo].positionX;
|
|
float toY = roadNodes[nodeTo].positionY;
|
|
|
|
float distance = sqrt(pow(toX - fromX, 2) + pow(toY - fromY, 2));
|
|
float course = atan2(toX - fromX, toY - fromY) * 180 / 3.14152965;
|
|
|
|
Connection connection;
|
|
connection.connectedPointNumber = nodeTo;
|
|
connection.distance = distance;
|
|
connection.course = course;
|
|
connection.speedLimit = speedLimit;
|
|
connection.motorway = motorway;
|
|
connection.tunnel = tunnel;
|
|
connection.againstOneWay = againstOneWay;
|
|
|
|
if (roadNodes[nodeFrom].connectionOne.connectedPointNumber == -1) {
|
|
roadNodes[nodeFrom].connectionOne = connection;
|
|
} else if (roadNodes[nodeFrom].connectionTwo.connectedPointNumber == -1) {
|
|
roadNodes[nodeFrom].connectionTwo = connection;
|
|
} else {
|
|
if (roadNodes[nodeFrom].extraConnections == NULL) {
|
|
roadNodes[nodeFrom].extraConnections = new std::vector<Connection>();
|
|
}
|
|
|
|
roadNodes[nodeFrom].extraConnections->push_back(connection);
|
|
}
|
|
}
|
|
|
|
void loadData(std::string filePath) {
|
|
set = RoadNodeSet();
|
|
|
|
std::ifstream source(filePath.c_str(), std::ios_base::binary);
|
|
char *buffer = new char[4];
|
|
source.read(buffer, 4);
|
|
uint32_t numberOfEntries = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
|
std::cout << "Reading " << numberOfEntries << " road nodes" << std::endl;
|
|
|
|
// Create the memory space for all these road nodes
|
|
set.numberOfNodes = numberOfEntries;
|
|
set.roadNodes = new RoadNode[numberOfEntries];
|
|
for (size_t i = 0; i < numberOfEntries; i++) {
|
|
// Each node in the file is of the type float x, float y, short z
|
|
source.read(buffer, 4);
|
|
// First, cast this to an int, reverse the byte order, and then cast to float
|
|
float positionX = getFloatFromBuffer(buffer);
|
|
source.read(buffer, 4);
|
|
float positionY = getFloatFromBuffer(buffer);
|
|
source.read(buffer, 2);
|
|
int positionZInt = ntohs(*reinterpret_cast<uint16_t*>(buffer));
|
|
float positionZ = (positionZInt - 30000) / 10.0;
|
|
|
|
set.roadNodes[i].positionX = positionX;
|
|
set.roadNodes[i].positionY = positionY;
|
|
set.roadNodes[i].positionZ = positionZ;
|
|
|
|
set.roadNodes[i].connectionOne.connectedPointNumber = -1;
|
|
set.roadNodes[i].connectionTwo.connectedPointNumber = -1;
|
|
|
|
set.roadNodes[i].extraConnections = NULL;
|
|
}
|
|
|
|
source.read(buffer, 4);
|
|
uint32_t numberOfLinks = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
|
std::cout << "Reading " << numberOfLinks << " links" << std::endl;
|
|
|
|
// Read all the links between nodes
|
|
for (size_t i = 0; i < numberOfLinks; i++) {
|
|
source.read(buffer, 4);
|
|
uint32_t fromPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
|
source.read(buffer, 4);
|
|
uint32_t toPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
|
|
|
source.read(buffer, 1);
|
|
uint8_t flagsByte = *reinterpret_cast<uint8_t*>(buffer);
|
|
|
|
uint8_t speedLimit = (flagsByte >> 4) * 10;
|
|
|
|
bool motorway = (flagsByte & 0x01) > 0;
|
|
bool tunnel = (flagsByte & 0x02) > 0;
|
|
bool passableSameDirection = (flagsByte & 0x04) > 0;
|
|
bool passableOppositeDirection = (flagsByte & 0x08) > 0;
|
|
|
|
addLinkToRoadNode(set.roadNodes, fromPoint, toPoint, speedLimit, motorway, tunnel, !passableSameDirection);
|
|
addLinkToRoadNode(set.roadNodes, toPoint, fromPoint, speedLimit, motorway, tunnel, !passableOppositeDirection);
|
|
}
|
|
|
|
delete[] buffer;
|
|
}
|
|
|
|
// Search functions
|
|
JSNodeInfo findClosestNode(float positionX, float positionY) {
|
|
float closestDistance = 1e99;
|
|
uint32_t closestNode = 0;
|
|
|
|
for (size_t i = 0; i < set.numberOfNodes; i++) {
|
|
RoadNode node = set.roadNodes[i];
|
|
float nodeX = node.positionX;
|
|
float nodeY = node.positionY;
|
|
|
|
float distance = sqrt(pow(positionX - nodeX, 2) + pow(positionY - nodeY, 2));
|
|
if (distance < closestDistance) {
|
|
closestDistance = distance;
|
|
closestNode = i;
|
|
}
|
|
}
|
|
|
|
JSNodeInfo result;
|
|
result.nodeId = closestNode;
|
|
result.positionX = set.roadNodes[closestNode].positionX;
|
|
result.positionY = set.roadNodes[closestNode].positionY;
|
|
result.positionZ = set.roadNodes[closestNode].positionZ;
|
|
|
|
return result;
|
|
}
|
|
|
|
float calculateSpeed(float startingSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
|
|
float slopeTan = heightDifference / horizontalDistance;
|
|
|
|
// If the slope is flat, that is one calculation
|
|
if (fabs(slopeTan) < 0.0001) {
|
|
return startingSpeed * exp(-dragCoefficient * horizontalDistance);
|
|
}
|
|
|
|
// If the slope is not flat, we should calculate some trig identities, and how long the slope is
|
|
float slope = atan(slopeTan);
|
|
float slopeSin = sin(slope);
|
|
float fullDistance = horizontalDistance * slopeTan / slopeSin;
|
|
|
|
// We need to calculate the terminal velocity given the slope we're in
|
|
float terminalVelocity = sqrt(fabs(GRAVITY_ACCELERATION * slopeSin) / dragCoefficient);
|
|
|
|
// First, calculate the final speed if we're going uphill
|
|
if (slope > 0) {
|
|
float timeToPeak = atan(startingSpeed / terminalVelocity) / (dragCoefficient * terminalVelocity);
|
|
float discriminant = exp(fullDistance * dragCoefficient) * cos(dragCoefficient * terminalVelocity * timeToPeak);
|
|
if (discriminant > 1.f) {
|
|
// If this value is greater than 1, it means that the slope is too steep and we can never reach the top with our
|
|
// starting speed
|
|
return -1;
|
|
}
|
|
return terminalVelocity * tan(acos(discriminant));
|
|
}
|
|
|
|
// Downhill must be split in three: starting slower than terminal velocity, starting at terminal velocity, and starting
|
|
// above terminal velocity
|
|
if (fabs(startingSpeed - terminalVelocity) < 0.0001) {
|
|
return terminalVelocity;
|
|
}
|
|
|
|
// If we're going faster than terminal velocity
|
|
if (startingSpeed > terminalVelocity) {
|
|
float k1 = log((startingSpeed - terminalVelocity) / (startingSpeed + terminalVelocity));
|
|
float tanhInput = asinh(exp(dragCoefficient * fullDistance)*sinh(k1 * 0.5));
|
|
return -terminalVelocity / tanh(tanhInput);
|
|
}
|
|
|
|
// We only get here if we're going slower than terminal velocity
|
|
float k1 = log((terminalVelocity + startingSpeed) / (terminalVelocity - startingSpeed));
|
|
float tanhInput = acosh(exp(dragCoefficient * fullDistance) * cosh(k1 * 0.5));
|
|
return terminalVelocity * tanh(tanhInput);
|
|
}
|
|
|
|
float calculateRequiredSpeed(float endSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
|
|
float slopeTan = heightDifference / horizontalDistance;
|
|
// If it's flat, the calculation is kinda simple
|
|
if (fabs(slopeTan) < 0.0001) {
|
|
return endSpeed * exp(horizontalDistance * dragCoefficient);
|
|
}
|
|
|
|
// If it's not flat, we're going to need the terminal velocity
|
|
float slope = atan(slopeTan);
|
|
float slopeSin = sin(slope);
|
|
float fullDistance = horizontalDistance * slopeTan / slopeSin;
|
|
float acceleration = -GRAVITY_ACCELERATION * slopeSin;
|
|
float terminalVelocity = sqrt(fabs(acceleration) / dragCoefficient);
|
|
|
|
// Uphill
|
|
if (slope > 0) {
|
|
float timeToPeak = acos(cos(atan(endSpeed / terminalVelocity))/exp(fullDistance * dragCoefficient)) / (terminalVelocity * dragCoefficient);
|
|
return terminalVelocity * tan(terminalVelocity * dragCoefficient * timeToPeak);
|
|
}
|
|
|
|
// Downhill, end speed equal to terminal velocity
|
|
if (fabs(endSpeed - terminalVelocity) < 0.001) {
|
|
return terminalVelocity;
|
|
}
|
|
|
|
// Downhill, end speed higher than terminal velocity
|
|
if (endSpeed > terminalVelocity) {
|
|
float k1 = asinh(exp(-fullDistance * dragCoefficient) * sinh(atanh(-terminalVelocity / endSpeed)));
|
|
return -terminalVelocity / tanh(k1);
|
|
}
|
|
|
|
// If we got here, we're going downhill with an end speed lower than terminal velocity
|
|
float discriminant = cosh(atanh(endSpeed / terminalVelocity)) / exp(fullDistance * dragCoefficient);
|
|
if (discriminant >= 1) {
|
|
return terminalVelocity * tanh(acosh(discriminant));
|
|
}
|
|
|
|
// If we got here, we will gain a higher speed than required even when starting from zero.
|
|
return 0;
|
|
}
|
|
|
|
void getNeighbourConnections(RoadNode node, Connection* targetArray, int &numberOfConnections) {
|
|
numberOfConnections = 0;
|
|
if (node.connectionOne.connectedPointNumber != -1) {
|
|
*(targetArray + numberOfConnections) = node.connectionOne;
|
|
numberOfConnections++;
|
|
}
|
|
if (node.connectionTwo.connectedPointNumber != -1) {
|
|
*(targetArray + numberOfConnections) = node.connectionTwo;
|
|
numberOfConnections++;
|
|
}
|
|
if (node.extraConnections != NULL) {
|
|
for (auto it = node.extraConnections->begin(); it != node.extraConnections->end(); it++) {
|
|
*(targetArray + numberOfConnections) = *it;
|
|
numberOfConnections++;
|
|
}
|
|
}
|
|
}
|
|
|
|
SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed, bool reverse) {
|
|
SearchResult result;
|
|
result.reverse = reverse;
|
|
result.startingNode = startingNode;
|
|
|
|
RoadNode firstNode = set.roadNodes[startingNode];
|
|
SearchNodeInfo firstNodeInfo;
|
|
firstNodeInfo.distanceFromPrevious = 0;
|
|
firstNodeInfo.currentSpeed = minimumSpeed;
|
|
result.reachableNodes[startingNode] = firstNodeInfo;
|
|
|
|
ListNode *nextNode = new ListNode;
|
|
|
|
nextNode->id = startingNode;
|
|
nextNode->currentSpeed = minimumSpeed;
|
|
nextNode->currentCourse = 0;
|
|
|
|
while (nextNode != NULL) {
|
|
ListNode *currentNode = nextNode;
|
|
nextNode = currentNode->next;
|
|
|
|
int currentId = currentNode->id;
|
|
float currentSpeed = currentNode->currentSpeed;
|
|
float currentCourse = currentNode->currentCourse;
|
|
|
|
RoadNode bestNode = set.roadNodes[currentId];
|
|
delete currentNode;
|
|
|
|
Connection neighbours[10];
|
|
int neighbourCounter = 0;
|
|
getNeighbourConnections(bestNode, neighbours, neighbourCounter);
|
|
|
|
for (int i = 0; i < neighbourCounter; i++) {
|
|
Connection neighbour = neighbours[i];
|
|
// First, if neighbour is in excluded area, skip it
|
|
if (excludedNodes.find(neighbour.connectedPointNumber) != excludedNodes.end()) {
|
|
continue;
|
|
}
|
|
|
|
if (neighbour.speedLimit > maximumSpeedLimit) {
|
|
continue;
|
|
}
|
|
|
|
if (neighbour.motorway && !allowMotorways) {
|
|
continue;
|
|
}
|
|
|
|
if (neighbour.tunnel && !allowTunnels) {
|
|
continue;
|
|
}
|
|
|
|
if (neighbour.againstOneWay && ! allowAgainstOneway) {
|
|
continue;
|
|
}
|
|
|
|
RoadNode neighbourNode = set.roadNodes[neighbour.connectedPointNumber];
|
|
float heightDifference = neighbourNode.positionZ - bestNode.positionZ;
|
|
float resultingSpeed = -1;
|
|
if (!reverse) {
|
|
resultingSpeed = calculateSpeed(currentSpeed, neighbour.distance, heightDifference, dragCoefficient);
|
|
if (resultingSpeed < minimumSpeed) {
|
|
continue;
|
|
}
|
|
resultingSpeed = fmin(resultingSpeed, maximumSpeed);
|
|
} else {
|
|
resultingSpeed = calculateRequiredSpeed(currentSpeed, neighbour.distance, -heightDifference, dragCoefficient);
|
|
if (resultingSpeed > maximumSpeed) {
|
|
continue;
|
|
}
|
|
resultingSpeed = fmax(resultingSpeed, minimumSpeed);
|
|
}
|
|
|
|
|
|
// If we limit the speed on corners, do that here
|
|
if (limitCornerSpeed) {
|
|
float courseDifference = fabs(currentCourse - neighbour.course);
|
|
if (courseDifference > 180.0) {
|
|
courseDifference = 360 - courseDifference;
|
|
}
|
|
|
|
float maximumCornerSpeed;
|
|
if (courseDifference > 95) {
|
|
maximumCornerSpeed = minimumSpeed;
|
|
} else if (courseDifference > 45.0) {
|
|
maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximumSpeed - minimumSpeed) + minimumSpeed;
|
|
} else {
|
|
maximumCornerSpeed = maximumSpeed;
|
|
}
|
|
|
|
if (reverse) {
|
|
if (resultingSpeed > maximumCornerSpeed) {
|
|
continue;
|
|
}
|
|
} else {
|
|
resultingSpeed = fmin(maximumCornerSpeed, resultingSpeed);
|
|
}
|
|
}
|
|
|
|
// Check if this node is already in the reachable nodes map
|
|
auto resultIterator = result.reachableNodes.find(neighbour.connectedPointNumber);
|
|
if (reverse) {
|
|
if (resultIterator != result.reachableNodes.end() && resultingSpeed >= resultIterator->second.currentSpeed) {
|
|
continue;
|
|
}
|
|
} else {
|
|
if (resultIterator != result.reachableNodes.end() && resultingSpeed <= resultIterator->second.currentSpeed) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
SearchNodeInfo reachableNodeInfo;
|
|
reachableNodeInfo.currentSpeed = resultingSpeed;
|
|
reachableNodeInfo.distanceFromPrevious = neighbour.distance;
|
|
result.reachableNodes[neighbour.connectedPointNumber] = reachableNodeInfo;
|
|
|
|
result.previous[neighbour.connectedPointNumber] = currentId;
|
|
|
|
ListNode *neighbourListNode = new ListNode;
|
|
neighbourListNode->id = neighbour.connectedPointNumber;
|
|
neighbourListNode->currentSpeed = reachableNodeInfo.currentSpeed;
|
|
neighbourListNode->currentCourse = neighbour.course;
|
|
|
|
if (reverse) {
|
|
if (nextNode == NULL || resultingSpeed > nextNode->currentSpeed) {
|
|
neighbourListNode->next = nextNode;
|
|
nextNode = neighbourListNode;
|
|
} else {
|
|
ListNode* previousSearchNode = nextNode;
|
|
ListNode* currentSearchNode = nextNode->next;
|
|
|
|
while (currentSearchNode != NULL && currentSearchNode->currentSpeed < resultingSpeed) {
|
|
previousSearchNode = currentSearchNode;
|
|
currentSearchNode = currentSearchNode->next;
|
|
}
|
|
|
|
previousSearchNode->next = neighbourListNode;
|
|
neighbourListNode->next = currentSearchNode;
|
|
}
|
|
} else {
|
|
if (nextNode == NULL || resultingSpeed < nextNode->currentSpeed) {
|
|
neighbourListNode->next = nextNode;
|
|
nextNode = neighbourListNode;
|
|
} else {
|
|
ListNode* previousSearchNode = nextNode;
|
|
ListNode* currentSearchNode = nextNode->next;
|
|
|
|
while(currentSearchNode != NULL && currentSearchNode->currentSpeed > resultingSpeed) {
|
|
previousSearchNode = currentSearchNode;
|
|
currentSearchNode = currentSearchNode->next;
|
|
}
|
|
|
|
previousSearchNode->next = neighbourListNode;
|
|
neighbourListNode->next = currentSearchNode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed, bool reverse) {
|
|
lastSearchResult = findAllPathsFromPoint(startingNode, minimumSpeed, maximumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway, limitCornerSpeed, reverse);
|
|
|
|
float startX = set.roadNodes[startingNode].positionX;
|
|
float startY = set.roadNodes[startingNode].positionY;
|
|
|
|
// Find all end points and sort them by distance
|
|
std::set<uint32_t> notEndPoints;
|
|
for (auto it = lastSearchResult.previous.begin(); it != lastSearchResult.previous.end(); it++) {
|
|
notEndPoints.insert(it->second);
|
|
}
|
|
|
|
std::vector<JSNodeInfo> farthestEndpoints;
|
|
for (auto it = lastSearchResult.reachableNodes.begin(); it != lastSearchResult.reachableNodes.end(); it++) {
|
|
if (notEndPoints.find(it->first) == notEndPoints.end()) {
|
|
JSNodeInfo entry;
|
|
entry.nodeId = it->first;
|
|
entry.positionX = set.roadNodes[entry.nodeId].positionX;
|
|
entry.positionY = set.roadNodes[entry.nodeId].positionY;
|
|
entry.distanceFromStart = sqrt(pow(entry.positionX - startX, 2) + pow(entry.positionY - startY, 2));
|
|
|
|
farthestEndpoints.push_back(entry);
|
|
}
|
|
}
|
|
|
|
std::sort(farthestEndpoints.begin(), farthestEndpoints.end(), [](JSNodeInfo first, JSNodeInfo second){return second.distanceFromStart < first.distanceFromStart;});
|
|
|
|
// Discard all points that are too close to each other
|
|
float minimumDistance = 300;
|
|
|
|
std::vector<JSNodeInfo> filteredEndpoints;
|
|
for (size_t i = 0; i < farthestEndpoints.size(); i++) {
|
|
float closestNode = 1e99;
|
|
JSNodeInfo currentNode = farthestEndpoints.at(i);
|
|
for (size_t j = 0; j < filteredEndpoints.size(); j++) {
|
|
JSNodeInfo otherNode = filteredEndpoints.at(j);
|
|
float distance = sqrt(pow(otherNode.positionX - currentNode.positionX, 2) + pow(otherNode.positionY - currentNode.positionY, 2));
|
|
if (distance < closestNode) {
|
|
closestNode = distance;
|
|
if (distance <= minimumDistance) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (closestNode > minimumDistance) {
|
|
filteredEndpoints.push_back(currentNode);
|
|
}
|
|
}
|
|
|
|
JSSearchResult searchResult;
|
|
searchResult.endPoints = filteredEndpoints;
|
|
searchResult.reverse = lastSearchResult.reverse;
|
|
return searchResult;
|
|
}
|
|
|
|
std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float minimumSpeed, float dragCoefficient) {
|
|
std::vector<JSNodeInfo> path;
|
|
|
|
if (lastSearchResult.startingNode != startingNode) {
|
|
return path;
|
|
}
|
|
|
|
if (lastSearchResult.reachableNodes.find(endNode) == lastSearchResult.reachableNodes.end()) {
|
|
return path;
|
|
}
|
|
|
|
std::vector<uint32_t> nodes;
|
|
uint32_t currentNode = endNode;
|
|
nodes.push_back(currentNode);
|
|
while (lastSearchResult.previous.find(currentNode) != lastSearchResult.previous.end()) {
|
|
currentNode = lastSearchResult.previous.find(currentNode)->second;
|
|
nodes.push_back(currentNode);
|
|
}
|
|
|
|
float currentDistance = 0;
|
|
|
|
for (auto it = nodes.rbegin(); it != nodes.rend(); it++) {
|
|
RoadNode roadNode = set.roadNodes[*it];
|
|
JSNodeInfo nodeInfo;
|
|
nodeInfo.nodeId = *it;
|
|
nodeInfo.positionX = roadNode.positionX;
|
|
nodeInfo.positionY = roadNode.positionY;
|
|
nodeInfo.positionZ = roadNode.positionZ;
|
|
|
|
SearchNodeInfo searchNodeInfo = lastSearchResult.reachableNodes.find(*it)->second;
|
|
nodeInfo.currentSpeed = searchNodeInfo.currentSpeed;
|
|
currentDistance += searchNodeInfo.distanceFromPrevious;
|
|
nodeInfo.distanceFromStart = currentDistance;
|
|
|
|
path.push_back(nodeInfo);
|
|
}
|
|
|
|
if (lastSearchResult.reverse) {
|
|
std::reverse(path.begin(), path.end());
|
|
}
|
|
|
|
float currentRequiredSpeed = -1.0;
|
|
for (auto it = path.rbegin(); it != path.rend(); it++) {
|
|
if (currentRequiredSpeed <= -1.0) {
|
|
it->requiredSpeed = minimumSpeed;
|
|
currentRequiredSpeed = minimumSpeed;
|
|
} else {
|
|
uint32_t currentNodeId = it->nodeId;
|
|
RoadNode currentNode = set.roadNodes[currentNodeId];
|
|
uint32_t nextNodeId = (it - 1)->nodeId;
|
|
RoadNode nextNode = set.roadNodes[nextNodeId];
|
|
|
|
float heightDifference = nextNode.positionZ - currentNode.positionZ;
|
|
|
|
Connection neighbours[10];
|
|
int numberOfNeighbours = 0;
|
|
getNeighbourConnections(currentNode, neighbours, numberOfNeighbours);
|
|
for (int i = 0; i < numberOfNeighbours; i++) {
|
|
Connection neighbour = neighbours[i];
|
|
if (neighbour.connectedPointNumber == nextNodeId) {
|
|
float horizontalDistance = neighbour.distance;
|
|
currentRequiredSpeed = fmax(calculateRequiredSpeed(currentRequiredSpeed, horizontalDistance, heightDifference, dragCoefficient), minimumSpeed);
|
|
it->requiredSpeed = currentRequiredSpeed;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
bool isInsideRing(float pointX, float pointY, std::vector<PolygonCoordinate> ring) {
|
|
// Draw a ray from the point directly towards the right, see if it crosses any line in the ring
|
|
int crossings = 0;
|
|
for (int i = 1; i < ring.size() + 1; i++) {
|
|
int currentPoint = i % ring.size();
|
|
int lastPoint = i - 1;
|
|
|
|
float lastY = ring.at(lastPoint).y;
|
|
float currentY = ring.at(currentPoint).y;
|
|
|
|
if (pointY > fmax(lastY, currentY) || pointY < fmin(lastY, currentY)) {
|
|
continue;
|
|
}
|
|
|
|
float lastX = ring.at(lastPoint).x;
|
|
float currentX = ring.at(currentPoint).x;
|
|
|
|
if (pointX > fmax(lastX, currentX)) {
|
|
continue;
|
|
}
|
|
|
|
if (pointX < fmin(lastX, currentX)) {
|
|
crossings += 1;
|
|
continue;
|
|
}
|
|
|
|
// If we don't go cleanly through the bounding box, we have to be a bit more smart about this
|
|
// We don't want to divide by zero. If the line is completely horizontal, we have a freak line which we will ignore
|
|
if (fabs(lastY - currentY) < 0.00001) {
|
|
continue;
|
|
}
|
|
|
|
float slope = (currentX - lastX) / (currentY - lastY);
|
|
float xAtPointY = lastX + (pointY - lastY) * slope;
|
|
|
|
if (xAtPointY >= pointX) {
|
|
crossings += 1;
|
|
}
|
|
}
|
|
|
|
return crossings % 2 == 1;
|
|
}
|
|
|
|
void getNodesWithinPolygons(std::vector<std::vector<std::vector<PolygonCoordinate>>> polygons, std::set<uint32_t> &resultSet) {
|
|
// Add new ones
|
|
for (auto polygon : polygons) {
|
|
// Get a bounding box for each polygon
|
|
float maxX = -1e99;
|
|
float minX = 1e99;
|
|
float maxY = -1e99;
|
|
float minY = 1e99;
|
|
|
|
for (auto ring : polygon) {
|
|
for (auto coordinate : ring) {
|
|
minX = fmin(minX, coordinate.x);
|
|
minY = fmin(minY, coordinate.y);
|
|
maxX = fmax(maxX, coordinate.x);
|
|
maxY = fmax(maxY, coordinate.y);
|
|
}
|
|
}
|
|
|
|
// Go through all nodes
|
|
for (size_t nodeId = 0; nodeId < set.numberOfNodes; nodeId++) {
|
|
RoadNode node = set.roadNodes[nodeId];
|
|
// If the node is outside the bounding box, just move on
|
|
if (node.positionX < minX || node.positionX > maxX || node.positionY < minY || node.positionY > maxY) {
|
|
continue;
|
|
}
|
|
// Otherwise, count how many times a ray straight east from the point crosses the polygon's rings
|
|
int crossings = 0;
|
|
for (auto ring : polygon) {
|
|
if (isInsideRing(node.positionX, node.positionY, ring)) {
|
|
crossings += 1;
|
|
}
|
|
}
|
|
|
|
if (crossings % 2 == 1) {
|
|
resultSet.insert(nodeId);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void excludeNodesWithinPolygons(std::vector<std::vector<std::vector<PolygonCoordinate>>> polygons) {
|
|
// Clear any old excluded nodes
|
|
excludedNodes.clear();
|
|
getNodesWithinPolygons(polygons, excludedNodes);
|
|
}
|
|
|
|
std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, std::vector<std::vector<std::vector<PolygonCoordinate>>> searchArea) {
|
|
std::set<uint32_t> allNodesWithinSearchArea;
|
|
getNodesWithinPolygons(searchArea, allNodesWithinSearchArea);
|
|
|
|
float minimumSlope = asin(dragCoefficient * minimumSpeed * minimumSpeed / GRAVITY_ACCELERATION);
|
|
float minimumSlopeTan = tan(minimumSlope);
|
|
|
|
std::vector<uint32_t> possibleStartNodes;
|
|
for (uint32_t nodeId : allNodesWithinSearchArea) {
|
|
if (excludedNodes.find(nodeId) != excludedNodes.end()) {
|
|
continue;
|
|
}
|
|
|
|
bool hasWayOut = false;
|
|
bool hasWayIn = false;
|
|
|
|
RoadNode node = set.roadNodes[nodeId];
|
|
Connection neighbours[10];
|
|
int numberOfNeighbours = 0;
|
|
getNeighbourConnections(node, neighbours, numberOfNeighbours);
|
|
|
|
for (int i = 0; i < numberOfNeighbours; i++) {
|
|
Connection connection = neighbours[i];
|
|
if (excludedNodes.find(connection.connectedPointNumber) != excludedNodes.end()) {
|
|
continue;
|
|
}
|
|
if (connection.motorway && !allowMotorways) {
|
|
continue;
|
|
}
|
|
if (connection.tunnel && !allowTunnels) {
|
|
continue;
|
|
}
|
|
if (connection.againstOneWay && !allowAgainstOneway) {
|
|
continue;
|
|
}
|
|
|
|
RoadNode neighbourNode = set.roadNodes[connection.connectedPointNumber];
|
|
float slopeTan = (neighbourNode.positionZ - node.positionZ) / connection.distance;
|
|
if (slopeTan > minimumSlopeTan) {
|
|
hasWayIn = true;
|
|
break;
|
|
} else if (slopeTan < -minimumSlopeTan) {
|
|
hasWayOut = true;
|
|
}
|
|
}
|
|
|
|
if (hasWayOut && !hasWayIn) {
|
|
// Insertion sort by height
|
|
possibleStartNodes.insert(
|
|
std::upper_bound(
|
|
possibleStartNodes.begin(),
|
|
possibleStartNodes.end(),
|
|
nodeId,
|
|
[](uint32_t nodeOne, uint32_t nodeTwo){return set.roadNodes[nodeOne].positionZ > set.roadNodes[nodeTwo].positionZ;}
|
|
),
|
|
nodeId
|
|
);
|
|
}
|
|
}
|
|
|
|
return possibleStartNodes;
|
|
}
|
|
|
|
AreaSearchResult startAreaSearch(float minimumSpeed, float maximumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed, std::vector<std::vector<std::vector<PolygonCoordinate>>> searchArea) {
|
|
currentAreaSearch = CurrentAreaSearch();
|
|
currentAreaSearch.startNodes = findPossibleStartNodes(minimumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway, searchArea);
|
|
currentAreaSearch.minimumSpeed = minimumSpeed;
|
|
currentAreaSearch.maximumSpeed = maximumSpeed;
|
|
currentAreaSearch.maximumSpeedLimit = maximumSpeedLimit;
|
|
currentAreaSearch.dragCoefficient = dragCoefficient;
|
|
currentAreaSearch.allowMotorways = allowMotorways;
|
|
currentAreaSearch.allowTunnels = allowTunnels;
|
|
currentAreaSearch.allowAgainstOneway = allowAgainstOneway;
|
|
currentAreaSearch.limitCornerSpeed = limitCornerSpeed;
|
|
|
|
AreaSearchResult result;
|
|
result.remainingNodes = currentAreaSearch.startNodes.size();
|
|
return result;
|
|
}
|
|
|
|
AreaSearchResult continueAreaSearch() {
|
|
uint32_t currentStartNode = currentAreaSearch.startNodes.at(currentAreaSearch.startNodeCounter);
|
|
SearchResult result = findAllPathsFromPoint(
|
|
currentStartNode,
|
|
currentAreaSearch.minimumSpeed,
|
|
currentAreaSearch.maximumSpeed,
|
|
currentAreaSearch.maximumSpeedLimit,
|
|
currentAreaSearch.dragCoefficient,
|
|
currentAreaSearch.allowMotorways,
|
|
currentAreaSearch.allowTunnels,
|
|
currentAreaSearch.allowAgainstOneway,
|
|
currentAreaSearch.limitCornerSpeed,
|
|
false
|
|
);
|
|
|
|
// Remove all nodes we have reached from here as possible future start nodes
|
|
auto startNodeIterator = currentAreaSearch.startNodes.begin() + currentAreaSearch.startNodeCounter + 1;
|
|
while (startNodeIterator != currentAreaSearch.startNodes.end()) {
|
|
uint32_t startNodeId = *startNodeIterator;
|
|
if (result.reachableNodes.find(startNodeId) != result.reachableNodes.end()) {
|
|
startNodeIterator = currentAreaSearch.startNodes.erase(startNodeIterator);
|
|
} else {
|
|
startNodeIterator++;
|
|
}
|
|
}
|
|
|
|
// Find the farthest reachable point from our current start point
|
|
std::set<uint32_t> notEndPoints;
|
|
for (auto it = result.previous.begin(); it != result.previous.end(); it++) {
|
|
notEndPoints.insert(it->second);
|
|
}
|
|
|
|
RoadNode startNode = set.roadNodes[currentStartNode];
|
|
float farthestDistance = 0;
|
|
uint32_t farthestNode = 0;
|
|
for (auto it = result.reachableNodes.begin(); it != result.reachableNodes.end(); it++) {
|
|
if (notEndPoints.find(it->first) == notEndPoints.end()) {
|
|
RoadNode endNode = set.roadNodes[it->first];
|
|
float distance = sqrt(pow(endNode.positionX - startNode.positionX, 2) + pow( endNode.positionY - startNode.positionY, 2));
|
|
if (distance > farthestDistance) {
|
|
farthestDistance = distance;
|
|
farthestNode = it->first;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::map<uint32_t, std::vector<uint32_t>> path;
|
|
path[farthestNode] = std::vector<uint32_t>();
|
|
uint32_t currentNode = farthestNode;
|
|
while (result.previous.find(currentNode) != result.previous.end()) {
|
|
currentNode = result.previous.find(currentNode)->second;
|
|
path[currentNode] = std::vector<uint32_t>();
|
|
}
|
|
|
|
// Check if our path overlaps too much with already travelled paths
|
|
std::map<uint32_t, std::vector<uint32_t>> overlap;
|
|
auto inserterIterator = overlap.begin();
|
|
std::set_intersection(
|
|
currentAreaSearch.utilizedNodes.begin(),
|
|
currentAreaSearch.utilizedNodes.end(),
|
|
path.begin(),
|
|
path.end(),
|
|
std::inserter(overlap, inserterIterator),
|
|
currentAreaSearch.utilizedNodes.value_comp()
|
|
);
|
|
|
|
std::map<uint32_t, uint32_t> overlapCounts;
|
|
for (auto it = overlap.begin(); it != overlap.end(); it++) {
|
|
for (auto startingNodeIt = it->second.begin(); startingNodeIt != it->second.end(); startingNodeIt++) {
|
|
uint32_t startingNode = *startingNodeIt;
|
|
if (overlapCounts.find(startingNode) == overlapCounts.end()) {
|
|
overlapCounts[startingNode] = 0;
|
|
}
|
|
overlapCounts[startingNode] = overlapCounts[startingNode] + 1;
|
|
}
|
|
}
|
|
|
|
auto overlapIterator = overlapCounts.begin();
|
|
while (overlapIterator != overlapCounts.end()) {
|
|
uint32_t overlapCount = overlapIterator->second;
|
|
if (overlapCount < path.size() * 0.5) {
|
|
overlapIterator = overlapCounts.erase(overlapIterator);
|
|
continue;
|
|
}
|
|
|
|
// In case we overlap more than 50% with another path, check if the current path is longer
|
|
uint32_t otherPath = overlapIterator->first;
|
|
bool isLongerThanOtherPath = true;
|
|
for (auto otherPathIterator = currentAreaSearch.currentAreaSearchResults.begin(); otherPathIterator != currentAreaSearch.currentAreaSearchResults.end(); otherPathIterator++) {
|
|
if (otherPathIterator->nodeId != otherPath) {
|
|
continue;
|
|
}
|
|
|
|
if (farthestDistance > otherPathIterator->longestRoute) {
|
|
currentAreaSearch.currentAreaSearchResults.erase(otherPathIterator);
|
|
break;
|
|
} else {
|
|
isLongerThanOtherPath = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (isLongerThanOtherPath) {
|
|
overlapIterator = overlapCounts.erase(overlapIterator);
|
|
continue;
|
|
}
|
|
|
|
// If we got here, we found another path that shares at least 50% of or route points, that is longer than the current route
|
|
break;
|
|
}
|
|
|
|
if (overlapCounts.size() == 0) {
|
|
AreaSearchEntry searchEntry;
|
|
searchEntry.nodeId = currentStartNode;
|
|
searchEntry.positionX = startNode.positionX;
|
|
searchEntry.positionY = startNode.positionY;
|
|
searchEntry.longestRoute = farthestDistance;
|
|
|
|
currentAreaSearch.currentAreaSearchResults.insert(
|
|
std::upper_bound(
|
|
currentAreaSearch.currentAreaSearchResults.begin(),
|
|
currentAreaSearch.currentAreaSearchResults.end(),
|
|
searchEntry,
|
|
[](AreaSearchEntry one, AreaSearchEntry two){return one.longestRoute > two.longestRoute;}
|
|
),
|
|
searchEntry
|
|
);
|
|
|
|
for (std::pair<uint32_t, std::vector<uint32_t>> pathNodeEntry : path) {
|
|
uint32_t pathNode = pathNodeEntry.first;
|
|
if (currentAreaSearch.utilizedNodes.find(pathNode) == currentAreaSearch.utilizedNodes.end()) {
|
|
currentAreaSearch.utilizedNodes[pathNode] = std::vector<uint32_t>();
|
|
}
|
|
currentAreaSearch.utilizedNodes[pathNode].push_back(currentStartNode);
|
|
}
|
|
}
|
|
|
|
currentAreaSearch.startNodeCounter++;
|
|
|
|
AreaSearchResult searchResult;
|
|
searchResult.remainingNodes = currentAreaSearch.startNodes.size() - currentAreaSearch.startNodeCounter;
|
|
searchResult.searchedNodes = currentAreaSearch.currentAreaSearchResults;
|
|
|
|
return searchResult;
|
|
}
|
|
|
|
#ifdef __EMSCRIPTEN__
|
|
EMSCRIPTEN_BINDINGS(my_module) {
|
|
emscripten::class_<JSNodeInfo>("NodeInfo")
|
|
.constructor<>()
|
|
.property("nodeId", &JSNodeInfo::nodeId)
|
|
.property("positionX", &JSNodeInfo::positionX)
|
|
.property("positionY", &JSNodeInfo::positionY)
|
|
.property("positionZ", &JSNodeInfo::positionZ)
|
|
.property("currentSpeed", &JSNodeInfo::currentSpeed)
|
|
.property("requiredSpeed", &JSNodeInfo::requiredSpeed)
|
|
.property("distanceFromStart", &JSNodeInfo::distanceFromStart);
|
|
|
|
emscripten::class_<JSSearchResult>("SearchResult")
|
|
.constructor<>()
|
|
.property("endPoints", &JSSearchResult::endPoints)
|
|
.property("reverse", &JSSearchResult::reverse);
|
|
|
|
emscripten::class_<PolygonCoordinate>("PolygonCoordinate")
|
|
.constructor<>()
|
|
.property("x", &PolygonCoordinate::x)
|
|
.property("y", &PolygonCoordinate::y);
|
|
|
|
emscripten::class_<AreaSearchEntry>("AreaSearchEntry")
|
|
.constructor<>()
|
|
.property("nodeId", &AreaSearchEntry::nodeId)
|
|
.property("positionX", &AreaSearchEntry::positionX)
|
|
.property("positionY", &AreaSearchEntry::positionY)
|
|
.property("longestRoute", &AreaSearchEntry::longestRoute);
|
|
|
|
emscripten::class_<AreaSearchResult>("AreaSearchResult")
|
|
.constructor<>()
|
|
.property("remainingNodes", &AreaSearchResult::remainingNodes)
|
|
.property("searchedNodes", &AreaSearchResult::searchedNodes);
|
|
|
|
emscripten::register_vector<JSNodeInfo>("NodeInfoArray");
|
|
emscripten::register_vector<PolygonCoordinate>("Ring");
|
|
emscripten::register_vector<std::vector<PolygonCoordinate>>("Polygon");
|
|
emscripten::register_vector<std::vector<std::vector<PolygonCoordinate>>>("MultiPolygon");
|
|
emscripten::register_vector<AreaSearchEntry>("AreaSearchEntries");
|
|
|
|
emscripten::function("loadData", &loadData);
|
|
emscripten::function("findClosestNode", &findClosestNode);
|
|
emscripten::function("findAllPathsFromPoint", &findAllPathsFromPointJS);
|
|
emscripten::function("getPath", &getPathJS);
|
|
emscripten::function("excludeNodesWithinPolygons", &excludeNodesWithinPolygons);
|
|
emscripten::function("startAreaSearch", &startAreaSearch);
|
|
emscripten::function("continueAreaSearch", &continueAreaSearch);
|
|
}
|
|
#endif |