Implement Sergey Makeev's lateral friction algorithm

adrenak · Sep 28, 2020 · d6e169f · d6e169f
1 parent fd3c1c3
commit d6e169f
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 13 deletions.
diff --git a/Assets/Tork/Runtime/Core/Motor.cs b/Assets/Tork/Runtime/Core/Motor.cs
@@ -39,16 +39,16 @@ void ApplyMotorTorque() {
             rs = radii[3] / total;
 
             if (ackermann.angle > 0) {
-                //ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fp;
-                //ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fs;
-                ackermann.RearRightWheel.MotorTorque = value * maxTorque * .5f; //rp;
-                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * .5f; //rp;
+                ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fp;
+                ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fs;
+                ackermann.RearRightWheel.MotorTorque = value * maxTorque * rp;
+                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * rs;
             }
             else {
-                //ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fp;
-                //ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fs;
-                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * .5f; //rp;
-                ackermann.RearRightWheel.MotorTorque = value * maxTorque * .5f; //rp;
+                ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fp;
+                ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fs;
+                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * rp;
+                ackermann.RearRightWheel.MotorTorque = value * maxTorque * rs;
             }
         }
 

diff --git a/Assets/Tork/Runtime/Core/TorkWheel.cs b/Assets/Tork/Runtime/Core/TorkWheel.cs
@@ -162,7 +162,7 @@ void FixedUpdate() {
                 transform.localEulerAngles.z
             );
             CalculateSuspension();
-            CalculateFriction();
+            CalculateFriction3();
             CalculateRPM();
         }
 
@@ -227,7 +227,7 @@ void CalculateFriction_Uncharted4() {
 
             var Favail = N * frictionCoeff;
             var Ff = MotorTorque / radius;
-            var Fl = Vl / Vf * N * frictionCoeff;
+            var Fl = Mathf.Abs(Vl) / Mathf.Abs(Vf) * N;
             var Ft = Mathf.Sqrt(Ff * Ff + Fl * Fl);
 
             var Fs = Favail / Ft;
@@ -243,7 +243,7 @@ void CalculateFriction_Uncharted4() {
             rigidbody.AddForceAtPosition(-Vector3.Project(Velocity, right).normalized * Flapplied, Hit.point);
             rigidbody.AddForceAtPosition(forward * Ffa * engineShaftToWheelRatio, Hit.point);
         }
-
+        #region APPROACH_2
         public float esslip;
         public float grip;
         public float drift;
@@ -285,5 +285,31 @@ void CalculateFriction() {
             rigidbody.AddForceAtPosition(-Vector3.Project(Velocity, right).normalized * Flapplied, Hit.point);
             rigidbody.AddForceAtPosition(forward * Ffa * engineShaftToWheelRatio, Hit.point);
         }
+        #endregion
+
+        void CalculateFriction3() {
+            if (!IsGrounded) return;
+
+            Vector3 right = transform.right;
+            Vector3 forward = transform.forward;
+            Vector3 Velocity = rigidbody.GetPointVelocity(Hit.point);
+
+            Vector3 lateralVelocity = Vector3.Project(Velocity, right);
+            Vector3 forwardVelocity = Vector3.Project(Velocity, forward);
+            Vector3 slip = (forwardVelocity + lateralVelocity) / 2;
+
+            float lateralFriction = Vector3.Project(right, slip).magnitude * SuspensionForce.magnitude / 9.8f / Time.fixedDeltaTime * sidewaysGrip;
+            rigidbody.AddForceAtPosition(-Vector3.Project(slip, lateralVelocity).normalized * lateralFriction, Hit.point);
+
+            float motorForce = MotorTorque / radius;
+            float maxForwardFriction = motorForce * forwardGrip;
+            float appliedForwardFriction = 0;
+            if (motorForce > 0)
+                appliedForwardFriction = Mathf.Clamp(motorForce, 0, maxForwardFriction);
+            else
+                appliedForwardFriction = Mathf.Clamp(motorForce, maxForwardFriction, 0);
+
+            rigidbody.AddForceAtPosition(forward * appliedForwardFriction * engineShaftToWheelRatio, Hit.point);
+        }
     }
 }
diff --git a/Assets/Tork/Samples/Scenes/Demo.unity b/Assets/Tork/Samples/Scenes/Demo.unity
@@ -4129,7 +4129,7 @@ PrefabInstance:
     - target: {fileID: 1214198107381208202, guid: 58b5a3e507c7b68449674674e18cab44,
         type: 3}
       propertyPath: range
-      value: 30
+      value: 45
       objectReference: {fileID: 0}
     - target: {fileID: 1954840075906366550, guid: 58b5a3e507c7b68449674674e18cab44,
         type: 3}
@@ -5014,7 +5014,7 @@ PrefabInstance:
     - target: {fileID: 1214198106691927564, guid: 58b5a3e507c7b68449674674e18cab44,
         type: 3}
       propertyPath: m_Enabled
-      value: 1
+      value: 0
       objectReference: {fileID: 0}
     - target: {fileID: 1214198106691927564, guid: 58b5a3e507c7b68449674674e18cab44,
         type: 3}