Friction curve approach

adrenak · Sep 27, 2020 · fd3c1c3 · fd3c1c3
1 parent 5a1d823
commit fd3c1c3
Show file tree

Hide file tree

Showing 4 changed files with 432 additions and 22 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 * rp;
-                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * rs;
+                //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;
             }
             else {
-                ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fp;
-                ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fs;
-                ackermann.RearLeftWheel.MotorTorque = value * maxTorque * rp;
-                ackermann.RearRightWheel.MotorTorque = value * maxTorque * rs;
+                //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;
             }
         }
 

diff --git a/Assets/Tork/Runtime/Core/TorkWheel.cs b/Assets/Tork/Runtime/Core/TorkWheel.cs
@@ -213,7 +213,7 @@ bool WheelRaycast(float maxDistance, ref RaycastHit hit) {
             return false;
         }
 
-        void CalculateFriction() {
+        void CalculateFriction_Uncharted4() {
             if (!IsGrounded) return;
 
             // Contact basis (can be different from wheel basis)
@@ -243,5 +243,47 @@ void CalculateFriction() {
             rigidbody.AddForceAtPosition(-Vector3.Project(Velocity, right).normalized * Flapplied, Hit.point);
             rigidbody.AddForceAtPosition(forward * Ffa * engineShaftToWheelRatio, Hit.point);
         }
+
+        public float esslip;
+        public float grip;
+        public float drift;
+
+        WheelFrictionCurve curve;
+
+        void CalculateFriction() {
+            if (!IsGrounded) return;
+
+            curve.extremumSlip = esslip;
+            curve.extremumValue = esslip * grip;
+            curve.asymptoteSlip = esslip * 1.5f;
+            curve.asymptoteValue = esslip * drift;
+
+            // Contact basis (can be different from wheel basis)
+            Vector3 right = transform.right;
+            Vector3 forward = transform.forward;
+            Vector3 Velocity = rigidbody.GetPointVelocity(Hit.point);
+
+            var Vf = Vector3.Project(Velocity, forward).magnitude;
+            var Vl = Vector3.Project(Velocity, right).magnitude;
+            var N = SuspensionForce.magnitude;
+
+            var Ff = MotorTorque / radius;
+            var Fl = curve.Evaluate(Vl);
+            Debug.Log(Vl);
+            //Debug.Log(Vl > curve.extremumSlip ? "Slipping" : "Gripping");
+
+            var Fs = 1;
+
+            var Flapplied = Fl * Fs;
+            var Ffmax = Ff * frictionCoeff;
+            float Ffa;
+            if (Ff > 0)
+                Ffa = Mathf.Clamp(Ff, 0, Ffmax);
+            else
+                Ffa = Mathf.Clamp(Ff, Ffmax, 0);
+
+            rigidbody.AddForceAtPosition(-Vector3.Project(Velocity, right).normalized * Flapplied, Hit.point);
+            rigidbody.AddForceAtPosition(forward * Ffa * engineShaftToWheelRatio, Hit.point);
+        }
     }
 }
diff --git a/Assets/Tork/Runtime/Utils/Extensions.cs b/Assets/Tork/Runtime/Utils/Extensions.cs
@@ -1,19 +1,27 @@
-using UnityEngine;
-
+using UnityEngine;
+
 namespace Adrenak.Tork {
-    public static class Extensions {
+    public static class Extensions {
         public static float GetCompressionRatio(this WheelCollider WheelL) {
             WheelHit hit;
             bool groundedL = WheelL.GetGroundHit(out hit);
             if(groundedL)
                 return 1 - ((-WheelL.transform.InverseTransformPoint(hit.point).y - WheelL.radius) / WheelL.suspensionDistance);
             return 0;
-        }
-
+        }
+
         public static WheelHit GetHit(this WheelCollider WheelL) {
             WheelHit hit;
             bool groundedL = WheelL.GetGroundHit(out hit);
             return hit;
-        }
+        }
+
+        public static float Evaluate(this WheelFrictionCurve curve, float slip) {
+            if (slip < curve.extremumSlip)
+                return slip * curve.extremumValue / curve.extremumSlip;
+            else
+                //return curve.asymptoteValue;
+                return ((slip - curve.asymptoteSlip) * (curve.extremumValue - curve.asymptoteValue) / (curve.extremumSlip - curve.asymptoteSlip)) + curve.asymptoteValue;
+        }
     }
-}
+}