Calculate required speeds analytically rather than by guessing

native/route_search.cpp

@@ -289,6 +289,47 @@ float calculate_speed(float starting_speed, float horizontal_distance, float hei
 	}
 }
 
+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.connection_one.connected_point_number != -1) {
@@ -478,47 +519,6 @@ JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, flo
 	return searchResult;
 }
 
-float calculateRequiredSpeed(float endSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
-	// The bike will never reach faster speeds than terminal velocity in free fall
-	float maximumSpeed = sqrt(GRAVITY_ACCELERATION / dragCoefficient);
-
-	// First, check if we'll reach the required speed no matter what
-	if (calculate_speed(0.00001, horizontalDistance, heightDifference, 0.0, maximumSpeed, dragCoefficient) >= endSpeed) {
-		return 0.0;
-	}
-
-	// Divide and conquer
-	float numerator = 0;
-	float denominator = 0.5;
-	float foundEndSpeed = -100.0;
-	int loopCounter = 0;
-
-	float closestFoundSpeed = 1e99;
-	float bestNumerator = 1;
-	float bestDenominator = 1;
-	do {
-		numerator *= 2;
-		denominator *= 2;
-		if (foundEndSpeed > endSpeed) {
-			numerator -= 1;
-		} else {
-			numerator += 1;
-		}
-		foundEndSpeed = calculate_speed(maximumSpeed * numerator / denominator, horizontalDistance, heightDifference, 0.01, 100.0, dragCoefficient);
-		loopCounter++;
-
-		if (fabs(foundEndSpeed - endSpeed) < closestFoundSpeed) {
-			closestFoundSpeed = foundEndSpeed;
-			bestNumerator = numerator;
-			bestDenominator = denominator;
-		}
-	} while (fabs(foundEndSpeed - endSpeed) > 0.013 && loopCounter < 32);
-
-	float requiredSpeed = maximumSpeed * bestNumerator / bestDenominator;
-
-	return requiredSpeed;
-}
-
 std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float minimumSpeed, float dragCoefficient) {
 	std::vector<JSNodeInfo> path;