Clean up naming in the C++ code
This commit is contained in:
Martin Asprusten
parent
46aacb2310
commit
2ca8a14d8f
@ -1,11 +1,16 @@
|
||||
#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;
|
||||
@ -13,23 +18,23 @@ const float GRAVITY_ACCELERATION = 9.81;
|
||||
|
||||
// Data structures
|
||||
struct Connection {
|
||||
int connected_point_number;
|
||||
int connectedPointNumber;
|
||||
float distance;
|
||||
float course;
|
||||
uint8_t speed_limit;
|
||||
uint8_t speedLimit;
|
||||
bool motorway;
|
||||
bool tunnel;
|
||||
bool againstOneWay;
|
||||
};
|
||||
|
||||
struct RoadNode{
|
||||
float position_x;
|
||||
float position_y;
|
||||
float position_z;
|
||||
float positionX;
|
||||
float positionY;
|
||||
float positionZ;
|
||||
|
||||
Connection connection_one;
|
||||
Connection connection_two;
|
||||
std::vector<Connection> *extra_connections;
|
||||
Connection connectionOne;
|
||||
Connection connectionTwo;
|
||||
std::vector<Connection> *extraConnections;
|
||||
};
|
||||
|
||||
struct RoadNodeSet {
|
||||
@ -64,8 +69,8 @@ struct JSSearchResult {
|
||||
|
||||
struct ListNode {
|
||||
int id;
|
||||
float current_speed;
|
||||
float current_course;
|
||||
float currentSpeed;
|
||||
float currentCourse;
|
||||
|
||||
ListNode* next = NULL;
|
||||
};
|
||||
@ -117,34 +122,34 @@ float getFloatFromBuffer(char* buffer) {
|
||||
return *((float*) &correctByteOrder);
|
||||
}
|
||||
|
||||
void addLinkToRoadNode(RoadNode* roadNodes, int node_from, int node_to, uint8_t speed_limit, bool motorway, bool tunnel, bool againstOneWay) {
|
||||
float from_x = roadNodes[node_from].position_x;
|
||||
float from_y = roadNodes[node_from].position_y;
|
||||
float to_x = roadNodes[node_to].position_x;
|
||||
float to_y = roadNodes[node_to].position_y;
|
||||
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(to_x - from_x, 2) + pow(to_y - from_y, 2));
|
||||
float course = atan2(to_x - from_x, to_y - from_y) * 180 / 3.14152965;
|
||||
float distance = sqrt(pow(toX - fromX, 2) + pow(toY - fromY, 2));
|
||||
float course = atan2(toX - fromX, toY - fromY) * 180 / 3.14152965;
|
||||
|
||||
Connection connection;
|
||||
connection.connected_point_number = node_to;
|
||||
connection.connectedPointNumber = nodeTo;
|
||||
connection.distance = distance;
|
||||
connection.course = course;
|
||||
connection.speed_limit = speed_limit;
|
||||
connection.speedLimit = speedLimit;
|
||||
connection.motorway = motorway;
|
||||
connection.tunnel = tunnel;
|
||||
connection.againstOneWay = againstOneWay;
|
||||
|
||||
if (roadNodes[node_from].connection_one.connected_point_number == -1) {
|
||||
roadNodes[node_from].connection_one = connection;
|
||||
} else if (roadNodes[node_from].connection_two.connected_point_number == -1) {
|
||||
roadNodes[node_from].connection_two = connection;
|
||||
if (roadNodes[nodeFrom].connectionOne.connectedPointNumber == -1) {
|
||||
roadNodes[nodeFrom].connectionOne = connection;
|
||||
} else if (roadNodes[nodeFrom].connectionTwo.connectedPointNumber == -1) {
|
||||
roadNodes[nodeFrom].connectionTwo = connection;
|
||||
} else {
|
||||
if (roadNodes[node_from].extra_connections == NULL) {
|
||||
roadNodes[node_from].extra_connections = new std::vector<Connection>();
|
||||
if (roadNodes[nodeFrom].extraConnections == NULL) {
|
||||
roadNodes[nodeFrom].extraConnections = new std::vector<Connection>();
|
||||
}
|
||||
|
||||
roadNodes[node_from].extra_connections->push_back(connection);
|
||||
roadNodes[nodeFrom].extraConnections->push_back(connection);
|
||||
}
|
||||
}
|
||||
|
||||
@ -154,73 +159,72 @@ void loadData(std::string filePath) {
|
||||
std::ifstream source(filePath.c_str(), std::ios_base::binary);
|
||||
char *buffer = new char[4];
|
||||
source.read(buffer, 4);
|
||||
uint32_t number_of_entries = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
std::cout << "Reading " << number_of_entries << " road nodes" << std::endl;
|
||||
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 = number_of_entries;
|
||||
set.roadNodes = new RoadNode[number_of_entries];
|
||||
for (size_t i = 0; i < number_of_entries; i++) {
|
||||
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 position_x = getFloatFromBuffer(buffer);
|
||||
float positionX = getFloatFromBuffer(buffer);
|
||||
source.read(buffer, 4);
|
||||
float position_y = getFloatFromBuffer(buffer);
|
||||
float positionY = getFloatFromBuffer(buffer);
|
||||
source.read(buffer, 2);
|
||||
int position_z_int = ntohs(*reinterpret_cast<uint16_t*>(buffer));
|
||||
float position_z = (position_z_int - 30000) / 10.0;
|
||||
int positionZInt = ntohs(*reinterpret_cast<uint16_t*>(buffer));
|
||||
float positionZ = (positionZInt - 30000) / 10.0;
|
||||
|
||||
set.roadNodes[i].position_x = position_x;
|
||||
set.roadNodes[i].position_y = position_y;
|
||||
set.roadNodes[i].position_z = position_z;
|
||||
set.roadNodes[i].positionX = positionX;
|
||||
set.roadNodes[i].positionY = positionY;
|
||||
set.roadNodes[i].positionZ = positionZ;
|
||||
|
||||
set.roadNodes[i].connection_one.connected_point_number = -1;
|
||||
set.roadNodes[i].connection_two.connected_point_number = -1;
|
||||
set.roadNodes[i].connectionOne.connectedPointNumber = -1;
|
||||
set.roadNodes[i].connectionTwo.connectedPointNumber = -1;
|
||||
|
||||
set.roadNodes[i].extra_connections = NULL;
|
||||
set.roadNodes[i].extraConnections = NULL;
|
||||
}
|
||||
|
||||
source.read(buffer, 4);
|
||||
uint32_t number_of_links = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
std::cout << "Reading " << number_of_links << " links" << std::endl;
|
||||
uint32_t numberOfLinks = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
std::cout << "Reading " << numberOfLinks << " links" << std::endl;
|
||||
|
||||
// Read all the links between nodes
|
||||
int connection_vectors = 0;
|
||||
for (size_t i = 0; i < number_of_links; i++) {
|
||||
for (size_t i = 0; i < numberOfLinks; i++) {
|
||||
source.read(buffer, 4);
|
||||
uint32_t from_point = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
uint32_t fromPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
source.read(buffer, 4);
|
||||
uint32_t to_point = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
uint32_t toPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
|
||||
|
||||
source.read(buffer, 1);
|
||||
uint8_t flags_byte = *reinterpret_cast<uint8_t*>(buffer);
|
||||
uint8_t flagsByte = *reinterpret_cast<uint8_t*>(buffer);
|
||||
|
||||
uint8_t speed_limit = (flags_byte >> 4) * 10;
|
||||
uint8_t speedLimit = (flagsByte >> 4) * 10;
|
||||
|
||||
bool motorway = (flags_byte & 0x01) > 0;
|
||||
bool tunnel = (flags_byte & 0x02) > 0;
|
||||
bool passable_same_direction = (flags_byte & 0x04) > 0;
|
||||
bool passable_opposite_direction = (flags_byte & 0x08) > 0;
|
||||
bool motorway = (flagsByte & 0x01) > 0;
|
||||
bool tunnel = (flagsByte & 0x02) > 0;
|
||||
bool passableSameDirection = (flagsByte & 0x04) > 0;
|
||||
bool passableOppositeDirection = (flagsByte & 0x08) > 0;
|
||||
|
||||
addLinkToRoadNode(set.roadNodes, from_point, to_point, speed_limit, motorway, tunnel, !passable_same_direction);
|
||||
addLinkToRoadNode(set.roadNodes, to_point, from_point, speed_limit, motorway, tunnel, !passable_opposite_direction);
|
||||
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 position_x, float position_y) {
|
||||
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 node_x = node.position_x;
|
||||
float node_y = node.position_y;
|
||||
float nodeX = node.positionX;
|
||||
float nodeY = node.positionY;
|
||||
|
||||
float distance = sqrt(pow(position_x - node_x, 2) + pow(position_y - node_y, 2));
|
||||
float distance = sqrt(pow(positionX - nodeX, 2) + pow(positionY - nodeY, 2));
|
||||
if (distance < closestDistance) {
|
||||
closestDistance = distance;
|
||||
closestNode = i;
|
||||
@ -229,63 +233,63 @@ JSNodeInfo findClosestNode(float position_x, float position_y) {
|
||||
|
||||
JSNodeInfo result;
|
||||
result.nodeId = closestNode;
|
||||
result.positionX = set.roadNodes[closestNode].position_x;
|
||||
result.positionY = set.roadNodes[closestNode].position_y;
|
||||
result.positionZ = set.roadNodes[closestNode].position_z;
|
||||
result.positionX = set.roadNodes[closestNode].positionX;
|
||||
result.positionY = set.roadNodes[closestNode].positionY;
|
||||
result.positionZ = set.roadNodes[closestNode].positionZ;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float calculate_speed(float starting_speed, float horizontal_distance, float height_difference, float minimum_speed, float maximum_speed, float drag_coefficient) {
|
||||
float slope_tan = height_difference / horizontal_distance;
|
||||
float final_speed = -1;
|
||||
float calculateSpeed(float startingSpeed, float horizontalDistance, float heightDifference, float minimumSpeed, float maximumSpeed, float dragCoefficient) {
|
||||
float slopeTan = heightDifference / horizontalDistance;
|
||||
float finalSpeed = -1;
|
||||
|
||||
// If the slope is flat, that is one calculation
|
||||
if (fabs(slope_tan) < 0.0001) {
|
||||
float time_to_finish = (exp(horizontal_distance * drag_coefficient) - 1) / (starting_speed * drag_coefficient);
|
||||
final_speed = starting_speed / (starting_speed * drag_coefficient * time_to_finish + 1);
|
||||
if (fabs(slopeTan) < 0.0001) {
|
||||
float timeToFinish = (exp(horizontalDistance * dragCoefficient) - 1) / (startingSpeed * dragCoefficient);
|
||||
finalSpeed = startingSpeed / (startingSpeed * dragCoefficient * timeToFinish + 1);
|
||||
} else {
|
||||
// Otherwise, we need to find some parameters
|
||||
float slope = atan(slope_tan);
|
||||
float slope_sin = sin(slope);
|
||||
float full_distance = horizontal_distance * slope_tan / slope_sin;
|
||||
float acceleration = -GRAVITY_ACCELERATION * slope_sin;
|
||||
float terminal_velocity = sqrt(fabs(acceleration) / drag_coefficient);
|
||||
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 time_to_peak = atan(starting_speed / terminal_velocity) / (drag_coefficient * terminal_velocity);
|
||||
float timeToPeak = atan(startingSpeed / terminalVelocity) / (dragCoefficient * terminalVelocity);
|
||||
// If the discriminant is greater than 1, the slope is so steep that we cannot reach the end with our starting speed
|
||||
float discriminant = cos(drag_coefficient * terminal_velocity * time_to_peak) * exp(full_distance * drag_coefficient);
|
||||
float discriminant = cos(dragCoefficient * terminalVelocity * timeToPeak) * exp(fullDistance * dragCoefficient);
|
||||
if (discriminant > 1.f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
float time_to_reach_end = time_to_peak - acos(discriminant) / (drag_coefficient * terminal_velocity);
|
||||
final_speed = terminal_velocity * tan(drag_coefficient * terminal_velocity * (time_to_peak - time_to_reach_end));
|
||||
float timeToReachEnd = timeToPeak - acos(discriminant) / (dragCoefficient * terminalVelocity);
|
||||
finalSpeed = terminalVelocity * tan(dragCoefficient * terminalVelocity * (timeToPeak - timeToReachEnd));
|
||||
} else {
|
||||
// Downhill
|
||||
// If the starting speed is very close to the terminal velocity, we'll just stay at terminal velocity
|
||||
if (fabs(starting_speed - terminal_velocity) < 0.001) {
|
||||
final_speed = terminal_velocity;
|
||||
} else if (starting_speed < terminal_velocity) {
|
||||
float k1 = terminal_velocity * log((terminal_velocity + starting_speed) / (terminal_velocity - starting_speed)) * 0.5;
|
||||
float k2 = -log(cosh(k1 / terminal_velocity)) / drag_coefficient;
|
||||
float time_spent = acosh(exp(drag_coefficient * (full_distance - k2))) / (drag_coefficient * terminal_velocity) - k1 / (drag_coefficient * pow(terminal_velocity, 2));
|
||||
final_speed = terminal_velocity * tanh(drag_coefficient * terminal_velocity * time_spent + k1 / terminal_velocity);
|
||||
} else if (starting_speed > terminal_velocity) {
|
||||
float k1 = log((starting_speed - terminal_velocity) / (starting_speed + terminal_velocity)) * terminal_velocity / 2;
|
||||
float k2 = -log(-sinh(k1 / terminal_velocity)) / drag_coefficient;
|
||||
float time_spent = k1 / (drag_coefficient * pow(terminal_velocity, 2)) - asinh(-exp(drag_coefficient * (full_distance - k2))) / (drag_coefficient * terminal_velocity);
|
||||
final_speed = -terminal_velocity / tanh(k1 / terminal_velocity - drag_coefficient * terminal_velocity * time_spent);
|
||||
if (fabs(startingSpeed - terminalVelocity) < 0.001) {
|
||||
finalSpeed = terminalVelocity;
|
||||
} else if (startingSpeed < terminalVelocity) {
|
||||
float k1 = terminalVelocity * log((terminalVelocity + startingSpeed) / (terminalVelocity - startingSpeed)) * 0.5;
|
||||
float k2 = -log(cosh(k1 / terminalVelocity)) / dragCoefficient;
|
||||
float timeSpent = acosh(exp(dragCoefficient * (fullDistance - k2))) / (dragCoefficient * terminalVelocity) - k1 / (dragCoefficient * pow(terminalVelocity, 2));
|
||||
finalSpeed = terminalVelocity * tanh(dragCoefficient * terminalVelocity * timeSpent + k1 / terminalVelocity);
|
||||
} else if (startingSpeed > terminalVelocity) {
|
||||
float k1 = log((startingSpeed - terminalVelocity) / (startingSpeed + terminalVelocity)) * terminalVelocity / 2;
|
||||
float k2 = -log(-sinh(k1 / terminalVelocity)) / dragCoefficient;
|
||||
float timeSpent = k1 / (dragCoefficient * pow(terminalVelocity, 2)) - asinh(-exp(dragCoefficient * (fullDistance - k2))) / (dragCoefficient * terminalVelocity);
|
||||
finalSpeed = -terminalVelocity / tanh(k1 / terminalVelocity - dragCoefficient * terminalVelocity * timeSpent);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (final_speed < minimum_speed) {
|
||||
if (finalSpeed < minimumSpeed) {
|
||||
return -1;
|
||||
} else {
|
||||
return std::fmin(final_speed, maximum_speed);
|
||||
return std::fmin(finalSpeed, maximumSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
@ -332,45 +336,45 @@ float calculateRequiredSpeed(float endSpeed, float horizontalDistance, float hei
|
||||
|
||||
void getNeighbourConnections(RoadNode node, Connection* targetArray, int &numberOfConnections) {
|
||||
numberOfConnections = 0;
|
||||
if (node.connection_one.connected_point_number != -1) {
|
||||
*(targetArray + numberOfConnections) = node.connection_one;
|
||||
if (node.connectionOne.connectedPointNumber != -1) {
|
||||
*(targetArray + numberOfConnections) = node.connectionOne;
|
||||
numberOfConnections++;
|
||||
}
|
||||
if (node.connection_two.connected_point_number != -1) {
|
||||
*(targetArray + numberOfConnections) = node.connection_two;
|
||||
if (node.connectionTwo.connectedPointNumber != -1) {
|
||||
*(targetArray + numberOfConnections) = node.connectionTwo;
|
||||
numberOfConnections++;
|
||||
}
|
||||
if (node.extra_connections != NULL) {
|
||||
for (auto it = node.extra_connections->begin(); it != node.extra_connections->end(); it++) {
|
||||
if (node.extraConnections != NULL) {
|
||||
for (auto it = node.extraConnections->begin(); it != node.extraConnections->end(); it++) {
|
||||
*(targetArray + numberOfConnections) = *it;
|
||||
numberOfConnections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float maximum_speed, int maximumSpeedLimit, float drag_coefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed) {
|
||||
SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed) {
|
||||
SearchResult result;
|
||||
result.startingNode = startingNode;
|
||||
|
||||
RoadNode firstNode = set.roadNodes[startingNode];
|
||||
SearchNodeInfo firstNodeInfo;
|
||||
firstNodeInfo.distanceFromPrevious = 0;
|
||||
firstNodeInfo.currentSpeed = minimum_speed;
|
||||
firstNodeInfo.currentSpeed = minimumSpeed;
|
||||
result.reachableNodes[startingNode] = firstNodeInfo;
|
||||
|
||||
ListNode *nextNode = new ListNode;
|
||||
|
||||
nextNode->id = startingNode;
|
||||
nextNode->current_speed = minimum_speed;
|
||||
nextNode->current_course = 0;
|
||||
nextNode->currentSpeed = minimumSpeed;
|
||||
nextNode->currentCourse = 0;
|
||||
|
||||
while (nextNode != NULL) {
|
||||
ListNode *currentNode = nextNode;
|
||||
nextNode = currentNode->next;
|
||||
|
||||
int currentId = currentNode->id;
|
||||
float currentSpeed = currentNode->current_speed;
|
||||
float currentCourse = currentNode->current_course;
|
||||
float currentSpeed = currentNode->currentSpeed;
|
||||
float currentCourse = currentNode->currentCourse;
|
||||
|
||||
RoadNode bestNode = set.roadNodes[currentId];
|
||||
delete currentNode;
|
||||
@ -382,11 +386,11 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
|
||||
for (int i = 0; i < neighbourCounter; i++) {
|
||||
Connection neighbour = neighbours[i];
|
||||
// First, if neighbour is in excluded area, skip it
|
||||
if (excludedNodes.find(neighbour.connected_point_number) != excludedNodes.end()) {
|
||||
if (excludedNodes.find(neighbour.connectedPointNumber) != excludedNodes.end()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (neighbour.speed_limit > maximumSpeedLimit) {
|
||||
if (neighbour.speedLimit > maximumSpeedLimit) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@ -402,9 +406,9 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
|
||||
continue;
|
||||
}
|
||||
|
||||
RoadNode neighbourNode = set.roadNodes[neighbour.connected_point_number];
|
||||
float heightDifference = neighbourNode.position_z - bestNode.position_z;
|
||||
float resultingSpeed = calculate_speed(currentSpeed, neighbour.distance, heightDifference, minimum_speed, maximum_speed, drag_coefficient);
|
||||
RoadNode neighbourNode = set.roadNodes[neighbour.connectedPointNumber];
|
||||
float heightDifference = neighbourNode.positionZ - bestNode.positionZ;
|
||||
float resultingSpeed = calculateSpeed(currentSpeed, neighbour.distance, heightDifference, minimumSpeed, maximumSpeed, dragCoefficient);
|
||||
|
||||
if (resultingSpeed < 0) {
|
||||
continue;
|
||||
@ -418,15 +422,15 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
|
||||
}
|
||||
|
||||
if (courseDifference > 95) {
|
||||
resultingSpeed = minimum_speed;
|
||||
resultingSpeed = minimumSpeed;
|
||||
} else if (courseDifference > 45.0) {
|
||||
float maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximum_speed - minimum_speed) + minimum_speed;
|
||||
float maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximumSpeed - minimumSpeed) + minimumSpeed;
|
||||
resultingSpeed = fmin(resultingSpeed, maximumCornerSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
// Check if this node is already in the reachable nodes map
|
||||
auto resultIterator = result.reachableNodes.find(neighbour.connected_point_number);
|
||||
auto resultIterator = result.reachableNodes.find(neighbour.connectedPointNumber);
|
||||
if (resultIterator != result.reachableNodes.end() && resultingSpeed <= resultIterator->second.currentSpeed) {
|
||||
continue;
|
||||
}
|
||||
@ -434,23 +438,23 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
|
||||
SearchNodeInfo reachableNodeInfo;
|
||||
reachableNodeInfo.currentSpeed = resultingSpeed;
|
||||
reachableNodeInfo.distanceFromPrevious = neighbour.distance;
|
||||
result.reachableNodes[neighbour.connected_point_number] = reachableNodeInfo;
|
||||
result.reachableNodes[neighbour.connectedPointNumber] = reachableNodeInfo;
|
||||
|
||||
result.previous[neighbour.connected_point_number] = currentId;
|
||||
result.previous[neighbour.connectedPointNumber] = currentId;
|
||||
|
||||
ListNode *neighbourListNode = new ListNode;
|
||||
neighbourListNode->id = neighbour.connected_point_number;
|
||||
neighbourListNode->current_speed = reachableNodeInfo.currentSpeed;
|
||||
neighbourListNode->current_course = neighbour.course;
|
||||
neighbourListNode->id = neighbour.connectedPointNumber;
|
||||
neighbourListNode->currentSpeed = reachableNodeInfo.currentSpeed;
|
||||
neighbourListNode->currentCourse = neighbour.course;
|
||||
|
||||
if (nextNode == NULL || resultingSpeed < nextNode->current_speed) {
|
||||
if (nextNode == NULL || resultingSpeed < nextNode->currentSpeed) {
|
||||
neighbourListNode->next = nextNode;
|
||||
nextNode = neighbourListNode;
|
||||
} else {
|
||||
ListNode* previousSearchNode = nextNode;
|
||||
ListNode* currentSearchNode = nextNode->next;
|
||||
|
||||
while(currentSearchNode != NULL && currentSearchNode->current_speed > resultingSpeed) {
|
||||
while(currentSearchNode != NULL && currentSearchNode->currentSpeed > resultingSpeed) {
|
||||
previousSearchNode = currentSearchNode;
|
||||
currentSearchNode = currentSearchNode->next;
|
||||
}
|
||||
@ -467,8 +471,8 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
|
||||
JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed) {
|
||||
lastSearchResult = findAllPathsFromPoint(startingNode, minimumSpeed, maximumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway, limitCornerSpeed);
|
||||
|
||||
float start_x = set.roadNodes[startingNode].position_x;
|
||||
float start_y = set.roadNodes[startingNode].position_y;
|
||||
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;
|
||||
@ -481,9 +485,9 @@ JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, flo
|
||||
if (notEndPoints.find(it->first) == notEndPoints.end()) {
|
||||
JSNodeInfo entry;
|
||||
entry.nodeId = it->first;
|
||||
entry.positionX = set.roadNodes[entry.nodeId].position_x;
|
||||
entry.positionY = set.roadNodes[entry.nodeId].position_y;
|
||||
entry.distanceFromStart = sqrt(pow(entry.positionX - start_x, 2) + pow(entry.positionY - start_y, 2));
|
||||
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);
|
||||
}
|
||||
@ -544,9 +548,9 @@ std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float
|
||||
RoadNode roadNode = set.roadNodes[*it];
|
||||
JSNodeInfo nodeInfo;
|
||||
nodeInfo.nodeId = *it;
|
||||
nodeInfo.positionX = roadNode.position_x;
|
||||
nodeInfo.positionY = roadNode.position_y;
|
||||
nodeInfo.positionZ = roadNode.position_z;
|
||||
nodeInfo.positionX = roadNode.positionX;
|
||||
nodeInfo.positionY = roadNode.positionY;
|
||||
nodeInfo.positionZ = roadNode.positionZ;
|
||||
|
||||
SearchNodeInfo searchNodeInfo = lastSearchResult.reachableNodes.find(*it)->second;
|
||||
nodeInfo.currentSpeed = searchNodeInfo.currentSpeed;
|
||||
@ -567,14 +571,14 @@ std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float
|
||||
uint32_t nextNodeId = (it - 1)->nodeId;
|
||||
RoadNode nextNode = set.roadNodes[nextNodeId];
|
||||
|
||||
float heightDifference = nextNode.position_z - currentNode.position_z;
|
||||
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.connected_point_number == nextNodeId) {
|
||||
if (neighbour.connectedPointNumber == nextNodeId) {
|
||||
float horizontalDistance = neighbour.distance;
|
||||
currentRequiredSpeed = fmax(calculateRequiredSpeed(currentRequiredSpeed, horizontalDistance, heightDifference, dragCoefficient), minimumSpeed);
|
||||
it->requiredSpeed = currentRequiredSpeed;
|
||||
@ -652,13 +656,13 @@ void getNodesWithinPolygons(std::vector<std::vector<std::vector<PolygonCoordinat
|
||||
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.position_x < minX || node.position_x > maxX || node.position_y < minY || node.position_y > maxY) {
|
||||
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.position_x, node.position_y, ring)) {
|
||||
if (isInsideRing(node.positionX, node.positionY, ring)) {
|
||||
crossings += 1;
|
||||
}
|
||||
}
|
||||
@ -699,7 +703,7 @@ std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSp
|
||||
|
||||
for (int i = 0; i < numberOfNeighbours; i++) {
|
||||
Connection connection = neighbours[i];
|
||||
if (excludedNodes.find(connection.connected_point_number) != excludedNodes.end()) {
|
||||
if (excludedNodes.find(connection.connectedPointNumber) != excludedNodes.end()) {
|
||||
continue;
|
||||
}
|
||||
if (connection.motorway && !allowMotorways) {
|
||||
@ -712,8 +716,8 @@ std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSp
|
||||
continue;
|
||||
}
|
||||
|
||||
RoadNode neighbourNode = set.roadNodes[connection.connected_point_number];
|
||||
float slopeTan = (neighbourNode.position_z - node.position_z) / connection.distance;
|
||||
RoadNode neighbourNode = set.roadNodes[connection.connectedPointNumber];
|
||||
float slopeTan = (neighbourNode.positionZ - node.positionZ) / connection.distance;
|
||||
if (slopeTan > minimumSlopeTan) {
|
||||
hasWayIn = true;
|
||||
break;
|
||||
@ -729,7 +733,7 @@ std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSp
|
||||
possibleStartNodes.begin(),
|
||||
possibleStartNodes.end(),
|
||||
nodeId,
|
||||
[](uint32_t nodeOne, uint32_t nodeTwo){return set.roadNodes[nodeOne].position_z > set.roadNodes[nodeTwo].position_z;}
|
||||
[](uint32_t nodeOne, uint32_t nodeTwo){return set.roadNodes[nodeOne].positionZ > set.roadNodes[nodeTwo].positionZ;}
|
||||
),
|
||||
nodeId
|
||||
);
|
||||
@ -793,7 +797,7 @@ AreaSearchResult continueAreaSearch() {
|
||||
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.position_x - startNode.position_x, 2) + pow( endNode.position_y - startNode.position_y, 2));
|
||||
float distance = sqrt(pow(endNode.positionX - startNode.positionX, 2) + pow( endNode.positionY - startNode.positionY, 2));
|
||||
if (distance > farthestDistance) {
|
||||
farthestDistance = distance;
|
||||
farthestNode = it->first;
|
||||
@ -869,8 +873,8 @@ AreaSearchResult continueAreaSearch() {
|
||||
if (overlapCounts.size() == 0) {
|
||||
AreaSearchEntry searchEntry;
|
||||
searchEntry.nodeId = currentStartNode;
|
||||
searchEntry.positionX = startNode.position_x;
|
||||
searchEntry.positionY = startNode.position_y;
|
||||
searchEntry.positionX = startNode.positionX;
|
||||
searchEntry.positionY = startNode.positionY;
|
||||
searchEntry.longestRoute = farthestDistance;
|
||||
|
||||
currentAreaSearch.currentAreaSearchResults.insert(
|
||||
@ -901,6 +905,7 @@ AreaSearchResult continueAreaSearch() {
|
||||
return searchResult;
|
||||
}
|
||||
|
||||
#ifdef __EMSCRIPTEN__
|
||||
EMSCRIPTEN_BINDINGS(my_module) {
|
||||
emscripten::class_<JSNodeInfo>("NodeInfo")
|
||||
.constructor<>()
|
||||
@ -947,3 +952,4 @@ EMSCRIPTEN_BINDINGS(my_module) {
|
||||
emscripten::function("startAreaSearch", &startAreaSearch);
|
||||
emscripten::function("continueAreaSearch", &continueAreaSearch);
|
||||
}
|
||||
#endif
|
||||
Loading…
x
Reference in New Issue
Block a user