feat: Add AutoDrive addon

Ackermann code refactored and cleaned up. Classes using Ackermann have
undergone changes

Assets/Tork/Runtime/Addons/AutoDrive.cs

@@ -3,49 +3,75 @@
 namespace Adrenak.Tork {
     public class AutoDrive : VehicleAddOn {
         public Vehicle vehicle;
-        public Transform destination;
-        public float rate = .5f;
-        public float minDistance;
-        public AnimationCurve accelerationVsDist = AnimationCurve.Linear(0, .5f, 50, 1f);
+        public Vector3 destination;
+        public float steeringRate = .5f;
+        public float brakingDistance;
 
-        public bool IsInReverseArea {
+        public float Direction {
             get {
-                var towards = destination.position - transform.position;
-                var locTowards = transform.InverseTransformDirection(towards);
+                var towards = destination - vehicle.transform.position;
+                var locTowards = vehicle.transform.InverseTransformDirection(towards);
 
-                var currMaxSteerAngle = Mathf.Abs(vehicle.Steering.range);
-                var separation = vehicle.Ackermann.AxleSeparation;
-                var width = vehicle.Ackermann.AxleWidth;
-                var currMinRadius = Ackermann.GetRadius(currMaxSteerAngle, separation, width);
-                var pivot = vehicle.transform.position + vehicle.transform.right * Mathf.Sign(locTowards.x) * currMinRadius;
+                // Whether the current destination is behind us
+                bool destinationIsBehind = locTowards.z < 0;
 
-                if (Vector3.Distance(pivot, destination.position) < currMinRadius)
-                    return true;
+                // The minimum turning radius of the vehicle at the given steering angle range
+                var minTurningDia = AckermannUtils.GetRadius(
+                    vehicle.Steering.range,
+                    vehicle.Ackermann.AxleSeparation,
+                    vehicle.Ackermann.AxleWidth
+                ) * 2;
 
-                bool isBehind = locTowards.z < 0;
-                if (isBehind && Vector3.Distance(pivot, destination.position) < currMinRadius * 2)
-                    return true;
+                // The center of the vehicle if it turns at the minimum turning radius right now
+                var minTurningRadiusCenter = vehicle.transform.position + vehicle.transform.right * Mathf.Sign(locTowards.x) * minTurningDia / 2;
 
-                return false;
+                // We have the min turning radius, but we create a "band" of turnign radius
+                // with an inner and an outer radii
+                float innerDia = minTurningDia - minTurningDia / 4;
+                float outerDia = minTurningDia + minTurningDia / 4;
+
+                // x is the distance between the center about which we will move if we decide to move
+                // with the smallest turning radius and the destination
+                float x = Vector3.Distance(minTurningRadiusCenter, destination);
+
+                // We certainly need to reverse as the inner radius is significantly smaller 
+                // than the actual min turnign radius itself and we will certainly not make it
+                // at min radius
+                if (x < innerDia / 2)
+                    return -1;
+
+                // We certainly can go in the forward direction because the outer radius
+                // is significantly larger than the actual min turning radius itself and we will
+                // certainly make it at the min radius
+                else if (x > outerDia / 2)
+                    return 1;
+
+                // 
+                else if (destinationIsBehind && x < innerDia)
+                    return -1;
+
+                else if (destinationIsBehind && x > outerDia)
+                    return 1;
+
+                else if (x < minTurningDia)
+                    return -(minTurningDia - x) / (minTurningDia - innerDia);
+                else
+                    return (x - minTurningDia) / (outerDia - minTurningDia);
             }
         }
 
         void Update() {
-            var distance = Vector3.Distance(vehicle.transform.position, destination.position);
-            
-            var towards = destination.position - transform.position;
+            var distance = Vector3.Distance(vehicle.transform.position, destination);
+
+            var towards = destination - transform.position;
             var locTowards = transform.InverseTransformDirection(towards);
             var reqAngle = Vector3.Angle(transform.forward, towards) * Mathf.Sign(locTowards.x);
-
-            var isAhead = locTowards.z > 0;
-
-            if (isAhead)
-                vehicle.Brake.value = 1 - (distance / minDistance);
-            else
-                vehicle.Brake.value = 0;
 
-            vehicle.Motor.value = accelerationVsDist.Evaluate(distance) * (IsInReverseArea ? -1 : 1);
-            vehicle.Steering.Angle = Mathf.Lerp(vehicle.Steering.Angle, reqAngle, rate) * (IsInReverseArea ? -1 : 1);
+            var direction = Direction;
+            var multiplier = Mathf.Clamp01(distance / brakingDistance);
+            vehicle.Brake.value = 1 - multiplier;
+            vehicle.Motor.value = direction;
+            vehicle.Steering.Angle = Mathf.Lerp(vehicle.Steering.Angle, reqAngle, steeringRate) * direction;
         }
     }
 }

Assets/Tork/Runtime/Core/Ackermann.cs

@@ -17,9 +17,7 @@ public class Ackermann : MonoBehaviour {
         [SerializeField] TorkWheel m_RearLeft;
         public TorkWheel RearLeftWheel { get { return m_RearLeft; } }
 
-        public float[,] Radii { get; private set; }
-
-        public float Angle { get; private set; }
+        public float Angle { get; set; }
 
         public float AxleSeparation {
             get { return (m_FrontLeft.transform.position - m_RearLeft.transform.position).magnitude; }
@@ -37,22 +35,13 @@ public float FrontLeftRadius {
             get { return AxleSeparation / Mathf.Sin(Mathf.Abs(m_FrontLeft.SteerAngle)); }
         }
 
-        private void Start() {
-            Radii = new float[2, 2];
-        }
-
-        private void Update() {
-            Radii = GetRadii(Angle, AxleSeparation, AxleWidth);
-        }
-
-        public void SetAngle(float angle) {
-            Angle = angle;
-            var farAngle = GetSecondaryAngle(angle, AxleWidth, AxleSeparation);
+        void Update() {
+            var farAngle = AckermannUtils.GetSecondaryAngle(Angle, AxleSeparation, AxleWidth);
 
             // The rear wheels are always at 0 steer in Ackermann
             m_RearLeft.SteerAngle = m_RearRight.SteerAngle = 0;
 
-            if (Mathf.Approximately(Angle, 0))
+            if (Mathf.Approximately((float)Angle, 0))
                 m_FrontRight.SteerAngle = m_FrontLeft.SteerAngle = 0;
             else if (Angle > 0) {
                 m_FrontRight.SteerAngle = Angle;
@@ -66,55 +55,13 @@ public void SetAngle(float angle) {
 
         public Vector3 GetPivot() {
             if (Angle > 0)
-                return m_FrontRight.transform.position + Radii[0, 1] * m_FrontRight.transform.right;
+                return m_RearRight.transform.position + CurrentRadii[0] * m_RearRight.transform.right;
             else
-                return m_FrontLeft.transform.position - Radii[0, 0] * m_FrontLeft.transform.right;
-        }
-
-        public float[,] CurrentRadii {
-            get { return GetRadii(Angle, AxleSeparation, AxleWidth); }
-        }
-
-        public float CurrentTurningRadius {
-            get {
-                var radii = CurrentRadii;
-                return (radii[0, 0] + radii[0, 1]) / 2;
-            }
+                return m_RearLeft.transform.position - CurrentRadii[0] * m_RearLeft.transform.right;
         }
 
-        public static float GetSecondaryAngle(float angle, float width, float separation) {
-            float close = separation / Mathf.Tan(Mathf.Abs(angle) * Mathf.Deg2Rad);
-            float far = close + width;
-
-            return Mathf.Atan(separation / far) * Mathf.Rad2Deg;
-        }
-
-        public static float GetRadius(float angle, float separation, float width){
-            var radii = GetRadii(angle, separation, width);
-            return radii[0, 0] + radii[0, 1] + radii[1, 0] + radii[1, 1] / 4;
-        }
-
-        public static float[,] GetRadii(float angle, float separation, float width) {
-            var secAngle = GetSecondaryAngle(angle, width, separation);
-            float[,] radii = new float[2, 2];
-
-            if (Mathf.Abs(angle) < 1)
-                radii[0, 0] = radii[1, 0] = radii[0, 1] = radii[1, 1] = 1000;
-
-            if (angle < -1) {
-                radii[0, 0] = separation / Mathf.Sin(Mathf.Abs(angle * Mathf.Deg2Rad));
-                radii[0, 1] = separation / Mathf.Sin(Mathf.Abs(secAngle * Mathf.Deg2Rad));
-                radii[1, 0] = separation / Mathf.Tan(Mathf.Abs(angle * Mathf.Deg2Rad));
-                radii[1, 1] = separation / Mathf.Tan(Mathf.Abs(secAngle * Mathf.Deg2Rad));
-            }
-            else if (angle > 1) {
-                radii[0, 0] = separation / Mathf.Sin(Mathf.Abs(secAngle * Mathf.Deg2Rad));
-                radii[0, 1] = separation / Mathf.Sin(Mathf.Abs(angle * Mathf.Deg2Rad));
-                radii[1, 0] = separation / Mathf.Tan(Mathf.Abs(secAngle * Mathf.Deg2Rad));
-                radii[1, 1] = separation / Mathf.Tan(Mathf.Abs(angle * Mathf.Deg2Rad));
-            }
-
-            return radii;
+        public float[] CurrentRadii {
+            get { return AckermannUtils.GetRadii(Angle, AxleSeparation, AxleWidth); }
         }
     }
 }

Assets/Tork/Runtime/Core/Brakes.cs

@@ -1,36 +1,40 @@
 using UnityEngine;
 
 namespace Adrenak.Tork {
-	public class Brakes : MonoBehaviour {
+    public class Brakes : MonoBehaviour {
         public Ackermann ackermann;
 
-		[Tooltip("The maximum braking torque that can be applied")]
-		[SerializeField] float maxTorque = 5000;
+        [Tooltip("The maximum braking torque that can be applied")]
+        [SerializeField] float maxTorque = 5000;
 
-		[Tooltip("Multiplier to the maxTorque [0..1]")]
-		public float value;
+        [Tooltip("Multiplier to the maxTorque [0..1]")]
+        public float value;
 
-		void FixedUpdate() {
+        void FixedUpdate() {
             value = Mathf.Clamp01(value);
 
-			float fr, fl, rr, rl;
+            float fs, fp, rs, rp;
 
             // If we have Ackerman steering, we apply torque based on the steering radius of each wheel
-            if (ackermann != null) {
-                var radii = Ackermann.GetRadii(ackermann.Angle, ackermann.AxleSeparation, ackermann.AxleWidth);
-                var total = radii[0, 0] + radii[1, 0] + radii[0, 1] + radii[1, 1];
-                fl = radii[0, 0] / total;
-                fr = radii[1, 0] / total;
-                rl = radii[0, 1] / total;
-                rr = radii[1, 1] / total;
-            }
-            else
-                fr = fl = rr = rl = .25f;
+            var radii = AckermannUtils.GetRadii(ackermann.Angle, ackermann.AxleSeparation, ackermann.AxleWidth);
+            var total = radii[0] + radii[1] + radii[2] + radii[3];
+            fp = radii[0] / total;
+            fs = radii[1] / total;
+            rp = radii[2] / total;
+            rs = radii[3] / total;
 
-			ackermann.FrontLeftWheel.BrakeTorque = value * maxTorque * fl;
-			ackermann.FrontRightWheel.BrakeTorque = value * maxTorque * fr;
-			ackermann.RearLeftWheel.BrakeTorque = value * maxTorque * rl;
-			ackermann.RearRightWheel.BrakeTorque = value * maxTorque * rr;
-		}
-	}
+            if (ackermann.Angle > 0) {
+                ackermann.FrontRightWheel.BrakeTorque = value * maxTorque * fp;
+                ackermann.FrontLeftWheel.BrakeTorque = value * maxTorque * fs;
+                ackermann.RearRightWheel.BrakeTorque = value * maxTorque * rp;
+                ackermann.RearLeftWheel.BrakeTorque = value * maxTorque * rs;
+            }
+            else {
+                ackermann.FrontLeftWheel.BrakeTorque = value * maxTorque * fp;
+                ackermann.FrontRightWheel.BrakeTorque = value * maxTorque * fs;
+                ackermann.RearLeftWheel.BrakeTorque = value * maxTorque * rp;
+                ackermann.RearRightWheel.BrakeTorque = value * maxTorque * rs;
+            }
+        }
+    }
 }

Assets/Tork/Runtime/Core/Motor.cs

@@ -2,36 +2,48 @@
 using UnityEngine;
 
 namespace Adrenak.Tork {
-	public class Motor : MonoBehaviour {
-		[Tooltip("The maximum torque that the motor generates")]
-		public float maxTorque = 10000;
+    public class Motor : MonoBehaviour {
+        [Tooltip("The maximum torque that the motor generates")]
+        public float maxTorque = 10000;
 
-		[Tooltip("Multiplier to the maxTorque")]
-		public float value;
+        [Tooltip("Multiplier to the maxTorque")]
+        public float value;
 
-		public float m_MaxReverseInput = -.5f;
+        public float m_MaxReverseInput = -.5f;
 
         public Ackermann ackermann;
 
-		void FixedUpdate() {
-			ApplyMotorTorque();
-		}
-
-		void ApplyMotorTorque() {
-			value = Mathf.Clamp(value, m_MaxReverseInput, 1);
-
-			// If we have Ackerman steering, we apply torque based on the steering radius of each wheel
-			var radii = Ackermann.GetRadii(ackermann.Angle, ackermann.AxleSeparation, ackermann.AxleWidth);
-			var total = radii[0, 0] + radii[1, 0] + radii[0, 1] + radii[1, 1];
-			var fl = radii[0, 0] / total;
-            var fr = radii[1, 0] / total;
-            var rl = radii[0, 1] / total;
-            var rr = radii[1, 1] / total;
-
-			ackermann.FrontLeftWheel.MotorTorque = value * maxTorque * fl;
-			ackermann.FrontRightWheel.MotorTorque = value * maxTorque * fr;
-			ackermann.RearLeftWheel.MotorTorque = value * maxTorque * rl;
-			ackermann.RearRightWheel.MotorTorque = value * maxTorque * rr;
-		}
-	}
+        void FixedUpdate() {
+            ApplyMotorTorque();
+        }
+
+        void Update() {
+            value = Mathf.Clamp(value, m_MaxReverseInput, 1);
+        }
+
+        void ApplyMotorTorque() {
+            float fs, fp, rs, rp;
+
+            // If we have Ackerman steering, we apply torque based on the steering radius of each wheel
+            var radii = AckermannUtils.GetRadii(ackermann.Angle, ackermann.AxleSeparation, ackermann.AxleWidth);
+            var total = radii[0] + radii[1] + radii[2] + radii[3];
+            fp = radii[0] / total;
+            fs = radii[1] / total;
+            rp = radii[2] / total;
+            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;
+            }
+            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;
+            }
+        }
+    }
 }

Assets/Tork/Runtime/Core/Steering.cs

@@ -20,7 +20,7 @@ void Update() {
             var destination = value * range;
             m_CurrAngle = Mathf.MoveTowards(m_CurrAngle, destination, Time.deltaTime * rate);
             m_CurrAngle = Mathf.Clamp(m_CurrAngle, -range, range);
-            ackermann.SetAngle(m_CurrAngle);
+            ackermann.Angle = m_CurrAngle;
         }
     }
 }

Assets/Tork/Runtime/Core/TorkWheel.cs

@@ -136,6 +136,8 @@ void Start() {
         }
 
         void FixedUpdate() {
+            Velocity = rigidbody.GetPointVelocity(Hit.point);
+
             transform.localEulerAngles = new Vector3(0, SteerAngle, 0);
             CalculateSuspension();
             CalculateFriction();
@@ -198,10 +200,9 @@ bool WheelRaycast(float maxDistance, ref RaycastHit nearestHit) {
         }
 
         void CalculateFriction() {
-            Velocity = rigidbody.GetPointVelocity(Hit.point);
-
             if (!IsGrounded) return;
 
+
             // Contact basis (can be different from wheel basis)
             Vector3 normal = Hit.normal;
             Vector3 side = transform.right;
@@ -212,8 +213,8 @@ void CalculateFriction() {
             var multiplier = Mathf.Cos(angle * Mathf.Deg2Rad);
 
             // Calculate sliding velocity (velocity without normal force)
-            Vector3 sideVel = Vector3.Dot(Velocity, side) * side * multiplier;
-            Vector3 forwardVel = Vector3.Dot(Velocity, forward) * forward * multiplier;
+            Vector3 sideVel = side * Vector3.Dot(Velocity, side) * multiplier;
+            Vector3 forwardVel = forward * Vector3.Dot(Velocity, forward) * multiplier;
             Vector3 planarVelocity2D = sideVel + forwardVel;
 
             Vector3 planarMomentum = planarVelocity2D * rigidbody.mass;
@@ -236,7 +237,6 @@ void CalculateFriction() {
             longForce -= brakeResistanceForce;
 
             gripResistanceForce -= longForce;
-
             rigidbody.AddForceAtPosition(gripResistanceForce, Hit.point);
             rigidbody.AddForceAtPosition(forward * MotorTorque / radius * forwardGrip * engineShaftToWheelRatio, Hit.point);
         }