*

2025-05-13 03:19:28 +03:00
parent e2847b060d
commit 762dcf4524
784 changed files with 542617 additions and 2 deletions
@@ -0,0 +1,169 @@
+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);*/
+
+    }
+}