adjust speed to gradient (uphill)

Assembly-CSharp-Editor.csproj

@@ -59,6 +59,7 @@
      <Compile Include="Assets\TutorialInfo\Scripts\Editor\ReadmeEditor.cs" />
      <Compile Include="Assets\Editor\MatchPlayerPosEditor.cs" />
      <Compile Include="Assets\Editor\Tests\QuartilesTest.cs" />
+     <Compile Include="Assets\Editor\Tests\BikePhysicsTest.cs" />
  <Reference Include="UnityEditor.TestRunner">
  <HintPath>C:/Unity Projects/VR Cycling/Library/ScriptAssemblies/UnityEditor.TestRunner.dll</HintPath>
  </Reference>

Assembly-CSharp.csproj

@@ -246,6 +246,7 @@
      <Compile Include="Assets\AdvancedAnt\Plugins\Ant\Fit\Profile\Types\Event.cs" />
      <Compile Include="Assets\AdvancedAnt\Plugins\Ant\Fit\Profile\Types\AttitudeValidity.cs" />
      <Compile Include="Assets\Scripts\Controller\SensorBikeController.cs" />
+     <Compile Include="Assets\Scripts\BicyclePhysics.cs" />
      <Compile Include="Assets\TutorialInfo\Scripts\Readme.cs" />
      <Compile Include="Assets\AdvancedAnt\Plugins\Ant\Fit\Profile\Types\AntNetwork.cs" />
      <Compile Include="Assets\AdvancedAnt\Plugins\Ant\FitTest.cs" />

Assets/Editor/Tests/BikePhysicsTest.cs

@@ -0,0 +1,60 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using AdditionalMathf;
+using NUnit.Framework;
+using UnityEditor;
+using UnityEngine;
+using UnityEngine.TestTools;
+
+public class BikePhysicsTest
+{
+    private const float DELTA = 1/3.6f;
+
+    [Test]
+    public void ZeroSlopeDoesntChangeSpeed()
+    {
+        const float speed = 30f/3.6f;
+        var slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, 80f, 0f);
+        Assert.AreEqual(speed, slopeSpeed, DELTA);
+    }
+
+    [Test]
+    public void PositiveGradientAffectsSpeed()
+    {
+        const float speed = 30f/3.6f;
+        const float mass = 80f;
+        
+        var grad = .05f;
+        var slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(16.34544/3.6f, slopeSpeed, DELTA);
+
+        grad = .1f;
+        slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(9.44704/3.6f, slopeSpeed, DELTA);
+        
+        grad = .15f;
+        slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(6.43698/3.6f, slopeSpeed, DELTA);
+    }
+    
+    [Test]
+    public void NegativeGradientAffectsSpeed()
+    {
+        const float speed = 30f/3.6f;
+        const float mass = 80f;
+        
+        var grad = -.05f;
+        var slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(44.5824/3.6f, slopeSpeed, DELTA);
+
+        grad = -.1f;
+        slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(56.91384/3.6f, slopeSpeed, DELTA);
+        
+        grad = -.15f;
+        slopeSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(speed, mass, grad);
+        Assert.AreEqual(67.43808/3.6f, slopeSpeed, DELTA);
+    }
+}

Assets/Editor/Tests/BikePhysicsTest.cs.meta

@@ -0,0 +1,3 @@
+fileFormatVersion: 2
+guid: b827854ce4ba4d738ae7e38419475e55
+timeCreated: 1614853034

Assets/Scripts/BicyclePhysics.cs

@@ -0,0 +1,65 @@
+using System;
+using UnityEngine;
+
+public class BicyclePhysics
+{
+    private const float K_A = 0.5f; //wind resistance coefficient
+    private const float A = 0.6f; //frontal area bike + rider; m2
+    private const float D = 1.226f; //air density; kg/m3
+
+    /// <summary>
+    /// Returns a speed in m/s for a bike riding a gradient with the same power it would be ridden in the flat with the input speed.
+    /// Using formula from http://www.sportsci.org/jour/9804/dps.html. Assuming v=s and neglecting rolling resistance
+    /// The speed is calculated by solving k_aAs_1^3d = k_aAs_2^3d + giMs_2 for s_2
+    /// </summary>
+    /// <param name="speedFlat">Speed the bike would have in flat terrain</param>
+    /// <param name="systemMass">Mass of bike + rider</param>
+    /// <param name="gradient">gradient in vertical meters gained per meter travelled forward</param>
+    /// <returns></returns>
+    public static float SpeedAtGradientForSpeedAtFlat(float speedFlat, float systemMass, float gradient)
+    {
+        var g = -Physics.gravity.y;
+
+        var divisor = Mathf.Pow(g * gradient * systemMass, 3f);
+        var dividend = 27f * Mathf.Pow(A * D * K_A, 3f);
+        Console.WriteLine($"Divisor: {divisor}");
+        Console.WriteLine($"Dividend: {dividend}");
+
+        var divisor2 = Mathf.Pow(speedFlat, 6f);
+        var dividend2 = 4f;
+        Console.WriteLine($"Divisor2: {divisor2}");
+        Console.WriteLine($"Dividend2: {dividend2}");
+        
+        var sqrtTerm = Mathf.Sqrt(
+            divisor
+            /
+            dividend
+            +
+            divisor2
+            /
+            dividend2
+        );
+        
+        Console.WriteLine($"Sqrt: {sqrtTerm}");
+
+        var divisor3 = Mathf.Pow(speedFlat, 3f);
+        var dividend3 = 2f;
+        Console.WriteLine($"Divisor3: {divisor3}");
+        Console.WriteLine($"Dividend3: {dividend3}");
+        
+        var secondTerm = divisor3 / dividend3;
+        
+        Console.WriteLine($"Second term: {secondTerm}");
+        Console.WriteLine($"SqrtTerm + secondTerm: {sqrtTerm+secondTerm}");
+
+        var firstqbrt = Qbrt(sqrtTerm + secondTerm);
+        var secondqbrt = Qbrt(-sqrtTerm + secondTerm);
+        
+        Console.WriteLine($"firstqbrt: {firstqbrt}");
+        Console.WriteLine($"secondqbrt: {secondqbrt}");
+        
+        return firstqbrt + secondqbrt;
+    }
+
+    private static float Qbrt(float x) => x < 0 ? -Mathf.Pow(-x, 1f/3f) : Mathf.Pow(x, 1f/3f);
+}

Assets/Scripts/BicyclePhysics.cs.meta

@@ -0,0 +1,3 @@
+fileFormatVersion: 2
+guid: 53a7818bb4324a3aa030ad659788310b
+timeCreated: 1614841337

Assets/Scripts/Controller/Bicycle/RbBicycleController.cs

@@ -1,5 +1,6 @@
 using System;
 using UnityEngine;
+using Wheels;
 
 namespace Controller.Bicycle
 {
@@ -7,19 +8,22 @@ namespace Controller.Bicycle
     {
         #region Variables
 
-        //private const float THRESHOLD_STANDING = 0.5f / 3.6f;
+        private const float THRESHOLD_GRADIENT_ZERO = .05f;
 
         private Transform rbTransform;
         private float currentSteerAngle;
         private float currentLeaningAngle;
-        private float currentSpeed;
+        private float currentSpeedSensed;
+        private float currentSpeedAdjusted;
+
+        [Header("Slope Impact")] public bool adjustSpeedToSlope = true;
 
         public Vector3 Forward => rbTransform.forward;
 
         public Vector3 Right => -rbTransform.right;
 
         public Vector3 Up => rbTransform.up;
-        
+
         public BicycleControllerMode ControllerMode
         {
             get => controllerMode;
@@ -28,10 +32,12 @@ namespace Controller.Bicycle
 
         public float CurrentSpeed
         {
-            get => currentSpeed;
-            set => currentSpeed = Mathf.Clamp(value, 0, maxSpeed);
+            get => adjustSpeedToSlope ? currentSpeedAdjusted : currentSpeedSensed;
+            set => currentSpeedSensed = Mathf.Clamp(value, 0, maxSpeed);
         }
 
+        public float CurrentSpeedKph => CurrentSpeed * 3.6f;
+
         public float CurrentSteerAngle
         {
             get => currentSteerAngle;
@@ -70,13 +76,45 @@ namespace Controller.Bicycle
 
         private void ApplyVelocity()
         {
-            var targetVelocity = new Vector3(0, 0, CurrentSpeed);
-            targetVelocity = rbTransform.TransformDirection(targetVelocity);
+            var targetVelocity = CalculateTargetVelocity();
+
             var velocityChange = targetVelocity - rigidBody.velocity;
             velocityChange.y = 0;
             rigidBody.AddForce(velocityChange, ForceMode.VelocityChange);
         }
 
+        private Vector3 CalculateTargetVelocity()
+        {
+            AdjustSpeedToGradientIfNeeded();
+            var tv = new Vector3(0, 0, currentSpeedAdjusted);
+            tv = rbTransform.TransformDirection(tv);
+            return tv;
+        }
+
+        private void AdjustSpeedToGradientIfNeeded()
+        {
+            var bikeAngle = rbTransform.localRotation.eulerAngles.x;
+            if (!adjustSpeedToSlope || Mathf.Abs(bikeAngle) <= THRESHOLD_GRADIENT_ZERO)
+            {
+                currentSpeedAdjusted = currentSpeedSensed;
+                return;
+            }
+
+            if (bikeAngle > 180)
+            {
+                bikeAngle -= 360f;
+            }
+            else if (bikeAngle < -180)
+            {
+                bikeAngle = 360f - bikeAngle;
+            }
+
+            var gradientDeg = -bikeAngle;
+            var gradient = Mathf.Tan(gradientDeg * Mathf.Deg2Rad);
+            currentSpeedAdjusted = gradient < 0 ? currentSpeedSensed * 1.5f : 
+                BicyclePhysics.SpeedAtGradientForSpeedAtFlat(currentSpeedSensed, rigidBody.mass, gradient); //TODO make work for downhill - and lerp between the speeds!
+        }
+
         private void ApplySteerAngleAndRotation()
         {
             //don't lean and rotate when veeeeeery sloooow/standing. Otherwise bike will rotate already

Assets/Scripts/Controller/KeyboardBikeController.cs

@@ -18,6 +18,8 @@ namespace Controller
         public float leaningAngleIncreasePerSecond = 2f;
         public float steeringAngleIncreasePerSecond = 2.5f;
 
+        private float sensedSpeed;
+
         private void Start()
         {
             bicycleController = GetComponent<IBicycleController>();
@@ -29,19 +31,23 @@ namespace Controller
             {
                 if (Input.GetKey(KeyCode.T))
                 {
-                    bicycleController.CurrentSpeed += speedIncreasePerSecond * Time.deltaTime;
+                    //sensedSpeed += speedIncreasePerSecond * Time.deltaTime;
+                    bicycleController.CurrentSpeed = 25 / 3.6f;
                 }
                 else if (bicycleController.CurrentSpeed > 0)
                 {
-                    bicycleController.CurrentSpeed = Mathf.Max(0,
-                        bicycleController.CurrentSpeed - speedDecreasePerSecond * Time.deltaTime);
+                    //sensedSpeed = Mathf.Max(0,
+                    //    sensedSpeed - speedDecreasePerSecond * Time.deltaTime);
                 }
 
                 if (Input.GetKey(KeyCode.G))
                 {
-                    bicycleController.CurrentSpeed = Mathf.Max(0,
-                        bicycleController.CurrentSpeed - brakeIncreasePerSecond * Time.deltaTime);
+                   // sensedSpeed = Mathf.Max(0,
+                    //    sensedSpeed - brakeIncreasePerSecond * Time.deltaTime);
+                    bicycleController.CurrentSpeed = 0f;
                 }
+
+               //bicycleController.CurrentSpeed = sensedSpeed;
             }
 
             if (steer)

Assets/Scripts/Display/BikeDataDisplay.cs

@@ -1,4 +1,5 @@
-using Sensors;
+using Controller.Bicycle;
+using Sensors;
 using TMPro;
 using UnityEngine;
 
@@ -19,7 +20,9 @@ namespace Display
 
         private BikeSensorData sensorData;
 
-        [Header("Speed")] public TextMeshProUGUI speedDisplay;
+        [Header("Speed")] public RbBicycleController bicycleController;
+        public TextMeshProUGUI speedDisplay;
+        public bool useSpeedFromBicycleController = true;
 
 
         private bool speedDisplayAvailable;
@@ -43,10 +46,11 @@ namespace Display
         // Update is called once per frame
         private void Update()
         {
-            //transform.Translate(Vector3.forward * (Time.deltaTime * (speedSensorReceiver?.Speed ?? 0)));
-            //bicycleController.SetSpeed(speedSensorReceiver?.Speed ?? 0);
-            //bicycleController.SetTorque(powerMeterReceiver?.CrankTorque ?? 0); //TODO crank to wheel torque;
-            if (speedDisplayAvailable) speedDisplay.text = $"{sensorData.SpeedData?.SpeedKmh ?? 0f:n2} km/h";
+            var bikeSpeed = useSpeedFromBicycleController
+                ? bicycleController.CurrentSpeedKph
+                : sensorData.SpeedData?.SpeedKmh ?? 0f;
+
+            if (speedDisplayAvailable) speedDisplay.text = $"{bikeSpeed:n2} km/h";
 
             if (powerDisplayAvailable) powerDisplay.text = $"{sensorData.PowermeterData?.InstantaneousPower ?? 0f:0} W";
 

Assets/Scripts/Helpers.cs

@@ -113,4 +113,5 @@ public class Helpers
 
         return usedIp;
     }
+
 }

Assets/Scripts/Wheels/LerpSlopeCollider.cs

@@ -12,9 +12,9 @@ namespace Wheels
         public Transform frontWheelContact;
         public Rigidbody bike;
 
-        private const float TOLERANCE = 0.03f;
+        private const float TOLERANCE = 0.005f;
         private const float MAX_RAYCAST_LENGTH = 2f;
-        
+
         private float distanceUntilCompletelyRotated;
         private Transform bikeTransform;
         private float currentFrontWheelSlope;
@@ -22,7 +22,6 @@ namespace Wheels
         private bool rotate;
         private int slopeDirection = 1;
 
-
         private void Start()
         {
             bikeTransform = bike.transform;
@@ -33,6 +32,7 @@ namespace Wheels
         {
             GUI.TextField(new Rect(10, 200, 200, 20), $"Direction = {slopeDirection}");
             GUI.TextField(new Rect(10, 220, 200, 20), $"MinY = {currentFrontWheelMinY}");
+            GUI.TextField(new Rect(10, 250, 200, 20), $"rotation.x = {bikeTransform.localRotation.eulerAngles.x:n2}");
             GUI.TextField(new Rect(10, 270, 200, 20), $"Slope = {currentFrontWheelSlope}");
         }