367 lines
10 KiB
C#
367 lines
10 KiB
C#
/*
|
|
using UnityEngine;
|
|
|
|
|
|
[ExecuteInEditMode]
|
|
public class Wheel : MonoBehaviour {
|
|
|
|
public Tyre tyre;
|
|
public Brake brake;
|
|
|
|
public DriveShaft _shaft;
|
|
|
|
public float discRadius = 0.385f; //15 inch by default
|
|
private float _totalWheelRadius;
|
|
|
|
public float TotalWheelRadius {
|
|
get {
|
|
return _totalWheelRadius;
|
|
}
|
|
}
|
|
|
|
public float _slipLon;
|
|
public float _slipLat;
|
|
private Vector3 _localVelocity;
|
|
private Vector3 _friction;
|
|
private Vector3 _peakSlip;
|
|
public float camber;
|
|
public bool DrawDebug = true;
|
|
public float weight = 50;
|
|
|
|
public float driveTorque = 0.0f;
|
|
public float brakeTorque = 0.0f;
|
|
|
|
|
|
public float brakingState = 0.0f;
|
|
private WheelContact _contact;
|
|
public Oscilloscope oscilloscope;
|
|
private int t;
|
|
|
|
public bool Grounded {
|
|
get {
|
|
return _contact.grounded;
|
|
}
|
|
}
|
|
|
|
public WheelContact Contact {
|
|
get {
|
|
return _contact;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
[ContextMenuItem("Reset coeffs", "resetPacejkaCoeffs")]
|
|
public PacejkaCoeffs _coeffs = new PacejkaCoeffs();
|
|
|
|
public float dtMul = 1;
|
|
|
|
private void resetPacejkaCoeffs() {
|
|
_coeffs = new PacejkaCoeffs();
|
|
}
|
|
|
|
public float AngularVelocity {
|
|
get { return _shaft.AngularVelocity; }
|
|
set {
|
|
_shaft.AngularVelocity = value;
|
|
}
|
|
}
|
|
|
|
// Use this for initialization
|
|
void Start () {
|
|
InitWheel();
|
|
float normalForce = 3.210758f * 1000.0f;
|
|
var lateralParams = Pacejka.LateralParams(normalForce, 0, _coeffs);
|
|
var longitudinalParams = Pacejka.LongitudinalParams(normalForce, _coeffs);
|
|
var aligningParams = Pacejka.AligningParams(normalForce, 0, _coeffs);
|
|
|
|
_peakSlip.x = Pacejka.Extremum(longitudinalParams);
|
|
_peakSlip.y = Pacejka.Extremum(lateralParams);
|
|
_peakSlip.z = Pacejka.Extremum(aligningParams);
|
|
|
|
if(oscilloscope != null)
|
|
{
|
|
oscilloscope.channels[0].sampler = Sampler0;
|
|
oscilloscope.channels[1].sampler = Sampler1;
|
|
oscilloscope.channels[2].sampler = Sampler2;
|
|
}
|
|
}
|
|
|
|
float Sampler0()
|
|
{
|
|
return _slipLat;
|
|
}
|
|
|
|
float Sampler1()
|
|
{
|
|
return localVelocity.x;
|
|
}
|
|
|
|
float Sampler2()
|
|
{
|
|
return 0;//Mathf.Repeat(_shaft.Angle, Mathf.PI * 2);
|
|
}
|
|
|
|
float Inertia{
|
|
get{return 1.0f / _shaft.Inv_inertia;}
|
|
set{_shaft.Inv_inertia = 1.0f / value;}
|
|
}
|
|
|
|
void InitWheel() {
|
|
if (_shaft == null) {
|
|
_shaft = new DriveShaft();
|
|
}
|
|
|
|
|
|
if (tyre != null) {
|
|
_totalWheelRadius = discRadius + tyre.height;
|
|
}
|
|
|
|
Inertia = weight*_totalWheelRadius*_totalWheelRadius*0.5f;
|
|
|
|
}
|
|
void OnValidate() {
|
|
InitWheel();
|
|
}
|
|
|
|
|
|
public void SetBraking(float amount) {
|
|
brakingState = Mathf.Clamp01(amount);
|
|
|
|
}
|
|
|
|
void SetTorque(float torque, float dt) {
|
|
_shaft.ApplyMomentum(torque * dt);
|
|
}
|
|
|
|
|
|
void OnDrawGizmosSelected() {
|
|
if (!DrawDebug) {
|
|
return;
|
|
}
|
|
var p = _coeffs;
|
|
|
|
Curve[] curves = new Curve[]{new Curve(), new Curve(), new Curve()};
|
|
|
|
int sampleCnt = 250;
|
|
float minArg = -100;
|
|
float maxArg = 100;
|
|
float argDiff = maxArg - minArg;
|
|
|
|
for (int i = 0; i < sampleCnt; ++i) {
|
|
var arg = minArg + argDiff / ((float)sampleCnt - 1) * i;
|
|
curves[0].AddArg(arg);
|
|
curves[1].AddArg(arg);
|
|
curves[2].AddArg(arg);
|
|
|
|
curves[0].AddVal(Pacejka.LongitudinalForce (5000.0f, arg, p));
|
|
curves[1].AddVal(Pacejka.LateralForce (5000.0f, camber, arg, p));
|
|
curves[2].AddVal(Pacejka.AligningForce (5000.0f, camber, arg, p));
|
|
}
|
|
|
|
float maxSlip = MathEx.FindExtremumValue(x => Pacejka.LongitudinalForce(5000.0f, x, p), 0, 100, 0.5f);
|
|
|
|
Tools.BeginGui ();
|
|
|
|
GUI.Label(new Rect(20,30,300,100), "Maximum wheel slip: " + maxSlip + " Value at that slip: " + Pacejka.LongitudinalForce(5000.0f, maxSlip, p));
|
|
Plot2D plot = new Plot2D(curves[0]);
|
|
|
|
plot.Title = "Longitudinal friction";
|
|
plot.OnGUI(new Rect(90,100,500,400));
|
|
|
|
camber = GUI.HorizontalSlider (new Rect (10, 10, 100, 10), camber, -10, 10);
|
|
Tools.EndGui ();
|
|
}
|
|
|
|
float GetTorque(float new_angvel, float dt) {
|
|
return _shaft.GetMomentum(new_angvel) / dt;
|
|
}
|
|
|
|
|
|
private float roll_resistance_lin = 0.00001f;
|
|
private float roll_resistance_quad = 0.00001f;
|
|
float getRollingResistance(
|
|
float velocity,
|
|
float resistance_factor)
|
|
{
|
|
// surface influence on rolling resistance
|
|
float rolling_resistance = resistance_factor * roll_resistance_lin;
|
|
|
|
// heat due to tire deformation increases rolling resistance
|
|
// approximate by quadratic function
|
|
rolling_resistance += velocity * velocity * roll_resistance_quad;
|
|
|
|
// rolling resistance direction
|
|
float resistance = -rolling_resistance;
|
|
if (velocity < 0) resistance = -resistance;
|
|
|
|
return resistance;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
int pp = 0;
|
|
private Vector3 lonForce;
|
|
private Vector3 latForce;
|
|
private float wheel_torque;
|
|
public float dampOsc = 0.85f;
|
|
public float fixedTimeBase = 0.02f;
|
|
private Vector3 localVelocity;
|
|
|
|
public void Integrate(float dt, Vector3 wheelWorldVelocity, float normal_force, out Vector3 force)
|
|
{
|
|
//normal_force = 5000.0f;
|
|
float wheel_drive_torque = driveTorque;
|
|
float patch_speed = AngularVelocity * _totalWheelRadius;
|
|
localVelocity = transform.InverseTransformDirection(wheelWorldVelocity);
|
|
|
|
tyre.Slip(patch_speed, localVelocity, out _slipLon, out _slipLat);
|
|
|
|
Vector3 friction_force = tyre.FrictionForce(normal_force, _slipLon, _slipLat, 0, _coeffs);
|
|
|
|
lonForce = transform.forward * friction_force[0];
|
|
latForce = -transform.right * friction_force[1];
|
|
|
|
Vector3 tire_force = lonForce + latForce;
|
|
|
|
float tire_friction_torque = friction_force[0] * _totalWheelRadius;
|
|
|
|
//calculate brake torque
|
|
float wheel_lock_torque = -AngularVelocity / dt * Inertia;
|
|
float wheel_brake_torque = wheel_lock_torque - wheel_drive_torque + tire_friction_torque;
|
|
float maxBreakTorque = brakeTorque;
|
|
if (wheel_brake_torque > 0 && wheel_brake_torque > maxBreakTorque)
|
|
{
|
|
wheel_brake_torque = maxBreakTorque;
|
|
}
|
|
else if (wheel_brake_torque < 0 && wheel_brake_torque < -maxBreakTorque)
|
|
{
|
|
wheel_brake_torque = -maxBreakTorque;
|
|
}
|
|
|
|
//limit the reaction torque to the applied drive and braking torque
|
|
float reaction_torque = tire_friction_torque;
|
|
float applied_torque = wheel_drive_torque + wheel_brake_torque;
|
|
if ( ( applied_torque > 0 && reaction_torque > applied_torque ) ||
|
|
( applied_torque < 0 && reaction_torque < applied_torque ) )
|
|
reaction_torque = applied_torque;
|
|
Vector3 tire_torque = Vector3.right * reaction_torque;// - direction::up * friction_force[2];
|
|
|
|
//set wheel torque due to tire rolling resistance
|
|
float rollResistanceCoefficient = 0;
|
|
float rolling_resistance = getRollingResistance(AngularVelocity, rollResistanceCoefficient);
|
|
float rolling_resistance_torque = rolling_resistance * _totalWheelRadius - tire_friction_torque;
|
|
|
|
|
|
wheel_torque = wheel_drive_torque + wheel_brake_torque + rolling_resistance_torque;
|
|
|
|
//have the wheels internally apply forces, or just forcibly set the wheel speed if the brakes are locked
|
|
|
|
float oldAngularVelocity = AngularVelocity;
|
|
SetTorque(wheel_torque, dt);
|
|
|
|
/ *float fixedTimeStepScalar = fixedTimeBase / Time.fixedDeltaTime;
|
|
float invFixedTimeStepScalar = 1.0f / fixedTimeStepScalar;
|
|
|
|
// damp low speed wheel oscillations
|
|
float deltaAngularVelocity = AngularVelocity - oldAngularVelocity;
|
|
float delta = deltaAngularVelocity * dampOsc * Mathf.Clamp01(invFixedTimeStepScalar);
|
|
AngularVelocity -= delta;* /
|
|
|
|
_shaft.Integrate(dt);
|
|
|
|
//viscous tire contact drag (hack)
|
|
float rollingDrag = 0.0f;
|
|
Vector3 wheel_drag = _localVelocity * rollingDrag;
|
|
|
|
|
|
force = Vector3.zero;
|
|
//apply forces to body
|
|
// Vector3 tire_pos = wheel_position[i] - body->getCenterOfMassPosition();
|
|
Vector3 world_tire_force = tire_force;
|
|
//Vector3 world_tire_torque = quatRotate ( wheel_orientation[i], tire_torque);
|
|
world_tire_force += wheel_drag;
|
|
//world_tire_torque += tire_pos.cross(world_tire_force);
|
|
force = world_tire_force;
|
|
|
|
|
|
//torque = torque + world_tire_torque;
|
|
}
|
|
|
|
float RPM
|
|
{
|
|
get {
|
|
return _shaft.AngularVelocity * 30.0f / 3.1415926535f;
|
|
}
|
|
}
|
|
|
|
public DriveShaft Shaft {
|
|
get {
|
|
return _shaft;
|
|
}
|
|
}
|
|
|
|
void OnGUI()
|
|
{
|
|
DrawLabels();
|
|
}
|
|
|
|
void DrawLabels() {
|
|
Label3D.AtWorldPosition(transform.position + Vector3.up * 1, "RPM: " + RPM);
|
|
Label3D.AtWorldPosition(transform.position + Vector3.up * 1.3f, "F: " + _friction.x + " S: " + _slipLon);
|
|
}
|
|
|
|
void OnDrawGizmos() {
|
|
|
|
if (tyre == null || _shaft == 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 radiusP0 = -forward * _totalWheelRadius;
|
|
var radiusP1 = forward * _totalWheelRadius * 1.2f;
|
|
radiusP0 = rollMatrix.MultiplyPoint3x4 (radiusP0);
|
|
radiusP1 = rollMatrix.MultiplyPoint3x4 (radiusP1);
|
|
Gizmos.DrawLine (wheelCenter + radiusP0, wheelCenter + radiusP1);
|
|
|
|
//draw wheel outer circle
|
|
AdvancedGizmo.DrawCircle(wheelCenter, right, _totalWheelRadius, Color.green, 36);
|
|
|
|
//draw wheel inner circle
|
|
AdvancedGizmo.DrawCircle(wheelCenter, right, 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);
|
|
|
|
}
|
|
}
|
|
*/
|