martin/Freewheeling
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make it possible to limit speed on sharp corners

Browse Source
This commit is contained in:
Martin Asprusten
2025-07-02 22:54:16 +02:00
parent d07c040d95
commit e5a4e9d716
6 changed files with 56 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
native/route_search.cpp
+30 -10
View File
@@ -15,6 +15,7 @@ const float GRAVITY_ACCELERATION = 9.81;
struct Connection {
int connected_point_number;
float distance;
float course;
uint8_t speed_limit;
bool motorway;
bool tunnel;
@@ -64,6 +65,7 @@ struct JSSearchResult {
struct ListNode {
int id;
float current_speed;
float current_course;
ListNode* next = NULL;
};
@@ -93,6 +95,7 @@ struct CurrentAreaSearch {
bool allowMotorways;
bool allowTunnels;
bool allowAgainstOneway;
bool limitCornerSpeed;
std::vector<uint32_t> startNodes;
size_t startNodeCounter = 0;
@@ -121,10 +124,12 @@ void addLinkToRoadNode(RoadNode* roadNodes, int node_from, int node_to, uint8_t
float to_y = roadNodes[node_to].position_y;
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;
Connection connection;
connection.connected_point_number = node_to;
connection.distance = distance;
connection.course = course;
connection.speed_limit = speed_limit;
connection.motorway = motorway;
connection.tunnel = tunnel;
@@ -302,7 +307,7 @@ void getNeighbourConnections(RoadNode node, Connection* targetArray, int &number
}
}
SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float maximum_speed, int maximumSpeedLimit, float drag_coefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway) {
SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float maximum_speed, int maximumSpeedLimit, float drag_coefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed) {
SearchResult result;
result.startingNode = startingNode;
@@ -313,10 +318,10 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
result.reachableNodes[startingNode] = firstNodeInfo;
ListNode *nextNode = new ListNode;
ListNode *lastNode = nextNode;
nextNode->id = startingNode;
nextNode->current_speed = minimum_speed;
nextNode->current_course = 0;
while (nextNode != NULL) {
ListNode *currentNode = nextNode;
@@ -324,12 +329,9 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
int currentId = currentNode->id;
float currentSpeed = currentNode->current_speed;
float currentCourse = currentNode->current_course;
RoadNode bestNode = set.roadNodes[currentId];
if (lastNode == currentNode) {
lastNode = NULL;
}
delete currentNode;
Connection neighbours[10];
@@ -366,6 +368,21 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
if (resultingSpeed < 0) {
continue;
}
// 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;
}
if (courseDifference > 95) {
resultingSpeed = minimum_speed;
} else if (courseDifference > 45.0) {
float maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximum_speed - minimum_speed) + minimum_speed;
resultingSpeed = fmin(resultingSpeed, maximumCornerSpeed);
}
}
// Check if this node is already in the reachable nodes map
auto resultIterator = result.reachableNodes.find(neighbour.connected_point_number);
@@ -383,6 +400,7 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
ListNode *neighbourListNode = new ListNode;
neighbourListNode->id = neighbour.connected_point_number;
neighbourListNode->current_speed = reachableNodeInfo.currentSpeed;
neighbourListNode->current_course = neighbour.course;
if (nextNode == NULL || resultingSpeed < nextNode->current_speed) {
neighbourListNode->next = nextNode;
@@ -405,8 +423,8 @@ SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float
return result;
}
JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway) {
lastSearchResult = findAllPathsFromPoint(startingNode, minimumSpeed, maximumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway);
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;
@@ -721,7 +739,7 @@ std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSp
return possibleStartNodes;
}
AreaSearchResult startAreaSearch(float minimumSpeed, float maximumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, std::vector<std::vector<std::vector<PolygonCoordinate>>> searchArea) {
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;
@@ -731,6 +749,7 @@ AreaSearchResult startAreaSearch(float minimumSpeed, float maximumSpeed, float m
currentAreaSearch.allowMotorways = allowMotorways;
currentAreaSearch.allowTunnels = allowTunnels;
currentAreaSearch.allowAgainstOneway = allowAgainstOneway;
currentAreaSearch.limitCornerSpeed = limitCornerSpeed;
AreaSearchResult result;
result.remainingNodes = currentAreaSearch.startNodes.size();
@@ -747,7 +766,8 @@ AreaSearchResult continueAreaSearch() {
currentAreaSearch.dragCoefficient,
currentAreaSearch.allowMotorways,
currentAreaSearch.allowTunnels,
currentAreaSearch.allowAgainstOneway
currentAreaSearch.allowAgainstOneway,
currentAreaSearch.limitCornerSpeed
);
// Remove all nodes we have reached from here as possible future start nodes