CityDrivingSimulator.Unity/Unity/Assets/Scripts/Car/WheelPhysics.cs

using UnityEngine;
using System.Collections;
using System.Net.Security;
using System;

public struct WheelContact {
    public RaycastHit hit;
    public bool hasHit;
    public bool grounded;
}

public class WheelPhysics : MonoBehaviour {
    public Tyre tyre;
    public Brake brake;

    public double driveTorque = 0.0f;
    public double brakeTorque = 0.0f;

    public double angularVelocity; //rad/sec
    public double inertiaMomentum = 3.45f;
    public double angle; //radian

    private WheelContact _contact;
    public bool Grounded {
        get {
            return _contact.grounded;
        }
    }

    public double discRadius = 0.385f; //15 inch by default

    public double TotalWheelRadius {
        get {
            return discRadius + tyre.height;
        }
    }

    public WheelContact Contact {
        get {
            return _contact;
        }
    }



    // Use this for initialization
    void Start () {
	
	}
	
	// Update is called once per frame
	void Update ()
	{
    
	}

  

    private double _slipLon;
    private double _slipLat;
    private Vector3 localVelocity;

    private int t = 0;

    public double reactionTorque;

    public void Integrate(double dt, Vector3 wheelWorldVelocity, double normal_force, out Vector3 force) {
     
        t++;
        localVelocity = transform.InverseTransformDirection(wheelWorldVelocity);
        double patchSpeed = angularVelocity * TotalWheelRadius;



        // tyre.Slip(patchSpeed, localVelocity, out _slipLon, out _slipLat);


        var Vc = new Double3(localVelocity.z, localVelocity.x, localVelocity.y);

        Double3 frictionForces = Pacejka.CalcPacejka(Vc, angularVelocity, TotalWheelRadius, TotalWheelRadius, normal_force, 0, tyre.pacejkaParams._coeffs2, true);
        frictionForces.y *= -1.0;
      //  Double3 frictionForces = tyre.FrictionForce(normal_force, _slipLon, _slipLat, 0, tyre.pacejkaParams._coeffs);

       /* if (name[name.Length - 2] == 'f' &&  t % 250 == 0) {
            Debug.LogFormat("Fz = {0} Fx = {1} Slip = {2} Vlon = {4} Name = {3}", normal_force, frictionForces.x, _slipLon, name, localVelocity.z);
        }*/


        double T_engine = driveTorque;
        double T_break = brakeTorque * -Math.Sign(angularVelocity);

        double T_input = T_engine + T_break;

        double T_reaction = frictionForces.x * TotalWheelRadius;

        reactionTorque = T_reaction;
        /*if (T_input < 0 && (T_reaction < T_input) || (T_input > 0 && (T_reaction > T_input)))
        {
            T_reaction = T_input;
        }*/


        double T_rolling_resist = tyre.GetRollingResistance(normal_force, TotalWheelRadius, localVelocity.z, 1.0f);

        double angularAcceleration = (T_engine - T_reaction + T_break - T_rolling_resist) /inertiaMomentum;
        angularVelocity += angularAcceleration * dt;
        //dw/dt = (T_engine + T_reaction - T_break - T_friction) / WheelInertia

        angle += angularVelocity * dt;

        Vector3 lonForce = transform.forward * (float)frictionForces.x;
        Vector3 latForce = -transform.right * (float)frictionForces.y;
        force = lonForce + latForce;
    }

    public void UpdateContactPoint(Vector3 up) {
        var raycastRay = new Ray(transform.position, -up);

        _contact.hasHit = false;
        _contact.grounded = false;
        if (Physics.Raycast(raycastRay, out _contact.hit)) {
            _contact.hasHit = true;
            if (_contact.hit.distance <= TotalWheelRadius) {
                _contact.grounded = true;
            }
        }
    }


    void OnDrawGizmos() {

        if (tyre == null) {
            return;
        }
        //DrawLabels();
        var forward = transform.forward;
        var right = transform.right;

        //draw wheel radius
        var wheelCenter = transform.position;
        //var rollMatrix = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(_shaft.Angle * Mathf.Rad2Deg, right), Vector3.one);
        var rollMatrix = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(0, right), Vector3.one);
        var radiusP0 = -forward * (float)TotalWheelRadius;
        var radiusP1 = forward * (float)(TotalWheelRadius * 1.2);
        radiusP0 = rollMatrix.MultiplyPoint3x4(radiusP0);
        radiusP1 = rollMatrix.MultiplyPoint3x4(radiusP1);
        Gizmos.DrawLine(wheelCenter + radiusP0, wheelCenter + radiusP1);

        //draw wheel outer circle
        AdvancedGizmo.DrawCircle(wheelCenter, right, (float)TotalWheelRadius, Color.green, 36);

        //draw wheel inner circle
        AdvancedGizmo.DrawCircle(wheelCenter, right, (float)discRadius, Color.green, 36);

        //draw forces
        /*Gizmos.color = Color.red;
        Gizmos.DrawLine(wheelCenter, wheelCenter + lonForce);

        Gizmos.color = Color.green;
        Gizmos.DrawLine(wheelCenter, wheelCenter + latForce);

        Gizmos.color = Color.blue;
        Gizmos.DrawLine(wheelCenter, wheelCenter + latForce + lonForce);

        Gizmos.color = Color.black;
        Gizmos.DrawLine(wheelCenter, wheelCenter + forward * 100);*/

    }
}