*
This commit is contained in:
@@ -0,0 +1,366 @@
|
||||
/*
|
||||
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);
|
||||
|
||||
}
|
||||
}
|
||||
*/
|
||||
Reference in New Issue
Block a user