Allow reverse search, where you find the farthest start point from a given end point
This commit is contained in:
Martin Asprusten
+120
-72
@@ -51,6 +51,7 @@ struct SearchResult {
|
||||
uint32_t startingNode;
|
||||
std::map<uint32_t, uint32_t> previous;
|
||||
std::map<uint32_t, SearchNodeInfo> reachableNodes;
|
||||
bool reverse;
|
||||
};
|
||||
|
||||
struct JSNodeInfo {
|
||||
@@ -65,6 +66,7 @@ struct JSNodeInfo {
|
||||
|
||||
struct JSSearchResult {
|
||||
std::vector<JSNodeInfo> endPoints;
|
||||
bool reverse;
|
||||
};
|
||||
|
||||
struct ListNode {
|
||||
@@ -240,57 +242,51 @@ JSNodeInfo findClosestNode(float positionX, float positionY) {
|
||||
return result;
|
||||
}
|
||||
|
||||
float calculateSpeed(float startingSpeed, float horizontalDistance, float heightDifference, float minimumSpeed, float maximumSpeed, float dragCoefficient) {
|
||||
float calculateSpeed(float startingSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
|
||||
float slopeTan = heightDifference / horizontalDistance;
|
||||
float finalSpeed = -1;
|
||||
|
||||
// If the slope is flat, that is one calculation
|
||||
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(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 = 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(dragCoefficient * terminalVelocity * timeToPeak) * exp(fullDistance * dragCoefficient);
|
||||
if (discriminant > 1.f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
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(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);
|
||||
}
|
||||
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));
|
||||
}
|
||||
|
||||
if (finalSpeed < minimumSpeed) {
|
||||
return -1;
|
||||
} else {
|
||||
return std::fmin(finalSpeed, maximumSpeed);
|
||||
|
||||
// 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) {
|
||||
@@ -352,8 +348,9 @@ void getNeighbourConnections(RoadNode node, Connection* targetArray, int &number
|
||||
}
|
||||
}
|
||||
|
||||
SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, 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, bool reverse) {
|
||||
SearchResult result;
|
||||
result.reverse = reverse;
|
||||
result.startingNode = startingNode;
|
||||
|
||||
RoadNode firstNode = set.roadNodes[startingNode];
|
||||
@@ -363,11 +360,11 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float m
|
||||
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;
|
||||
@@ -408,12 +405,22 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float m
|
||||
|
||||
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;
|
||||
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);
|
||||
@@ -421,18 +428,34 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float m
|
||||
courseDifference = 360 - courseDifference;
|
||||
}
|
||||
|
||||
float maximumCornerSpeed;
|
||||
if (courseDifference > 95) {
|
||||
resultingSpeed = minimumSpeed;
|
||||
maximumCornerSpeed = minimumSpeed;
|
||||
} else if (courseDifference > 45.0) {
|
||||
float maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximumSpeed - minimumSpeed) + minimumSpeed;
|
||||
resultingSpeed = fmin(resultingSpeed, maximumCornerSpeed);
|
||||
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 (resultIterator != result.reachableNodes.end() && resultingSpeed <= resultIterator->second.currentSpeed) {
|
||||
continue;
|
||||
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;
|
||||
@@ -447,20 +470,38 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float m
|
||||
neighbourListNode->currentSpeed = reachableNodeInfo.currentSpeed;
|
||||
neighbourListNode->currentCourse = neighbour.course;
|
||||
|
||||
if (nextNode == NULL || resultingSpeed < nextNode->currentSpeed) {
|
||||
neighbourListNode->next = nextNode;
|
||||
nextNode = neighbourListNode;
|
||||
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 {
|
||||
ListNode* previousSearchNode = nextNode;
|
||||
ListNode* currentSearchNode = nextNode->next;
|
||||
|
||||
while(currentSearchNode != NULL && currentSearchNode->currentSpeed > resultingSpeed) {
|
||||
previousSearchNode = currentSearchNode;
|
||||
currentSearchNode = currentSearchNode->next;
|
||||
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;
|
||||
}
|
||||
|
||||
previousSearchNode->next = neighbourListNode;
|
||||
neighbourListNode->next = currentSearchNode;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -468,8 +509,8 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float m
|
||||
return result;
|
||||
}
|
||||
|
||||
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);
|
||||
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;
|
||||
@@ -520,6 +561,7 @@ JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, flo
|
||||
|
||||
JSSearchResult searchResult;
|
||||
searchResult.endPoints = filteredEndpoints;
|
||||
searchResult.reverse = lastSearchResult.reverse;
|
||||
return searchResult;
|
||||
}
|
||||
|
||||
@@ -560,6 +602,10 @@ std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float
|
||||
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) {
|
||||
@@ -771,7 +817,8 @@ AreaSearchResult continueAreaSearch() {
|
||||
currentAreaSearch.allowMotorways,
|
||||
currentAreaSearch.allowTunnels,
|
||||
currentAreaSearch.allowAgainstOneway,
|
||||
currentAreaSearch.limitCornerSpeed
|
||||
currentAreaSearch.limitCornerSpeed,
|
||||
false
|
||||
);
|
||||
|
||||
// Remove all nodes we have reached from here as possible future start nodes
|
||||
@@ -919,7 +966,8 @@ EMSCRIPTEN_BINDINGS(my_module) {
|
||||
|
||||
emscripten::class_<JSSearchResult>("SearchResult")
|
||||
.constructor<>()
|
||||
.property("endPoints", &JSSearchResult::endPoints);
|
||||
.property("endPoints", &JSSearchResult::endPoints)
|
||||
.property("reverse", &JSSearchResult::reverse);
|
||||
|
||||
emscripten::class_<PolygonCoordinate>("PolygonCoordinate")
|
||||
.constructor<>()
|
||||
|
||||
Reference in New Issue
Block a user