415 lines
12 KiB
C#
415 lines
12 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
|
|
/*
|
|
public class CarWheelHit{
|
|
public Vector3 point;
|
|
public Vector3 normal;
|
|
}
|
|
|
|
public class CarWheelCollider : MonoBehaviour {
|
|
public float wheelRadius = 0.33f;
|
|
public float _steerAngle;
|
|
|
|
public bool DrawDebug = true;
|
|
[ContextMenuItem("Reset coeffs", "resetPacejkaCoeffs")]
|
|
public PacejkaCoeffs _coeffs = new PacejkaCoeffs();
|
|
|
|
|
|
private Rigidbody _chassisRigidBody;
|
|
*/
|
|
/*private CarWheelHit _hit = new CarWheelHit();
|
|
private bool _isGrounded;
|
|
private Vector3 _relativePosition; //Relative to car body
|
|
private Vector3 _velocity;
|
|
private Vector3 _localVelocity;
|
|
public float camber;
|
|
private Vector3 _friction = new Vector3 ();
|
|
public float _appliedTorque;
|
|
private Vector2 _slip; //X = lateral, Y = longitudinal
|
|
private Vector3 _peakSlip;
|
|
private float _driveTorque;
|
|
private float _breakTorque;*/
|
|
|
|
/*
|
|
private DriveShaft _driveShaft = new DriveShaft();
|
|
|
|
|
|
private void resetPacejkaCoeffs() {
|
|
_coeffs = new PacejkaCoeffs();
|
|
}
|
|
*/
|
|
|
|
/*public float SteerAngle {
|
|
get {
|
|
return _steerAngle;
|
|
}
|
|
set {
|
|
_steerAngle = value;
|
|
}
|
|
}*/
|
|
|
|
/*void SetTorque(float torque, float dt){
|
|
_driveShaft.applyMomentum(torque * dt);
|
|
}
|
|
|
|
float RPM{
|
|
get{return _driveShaft.AngularVelocity * 30.0f / 3.141593f;}
|
|
}
|
|
|
|
void Integrate(float dt){
|
|
_driveShaft.integrate(dt);
|
|
}
|
|
|
|
private void UpdateRelativePosition(){
|
|
_relativePosition = _chassisRigidBody.transform.InverseTransformPoint (transform.position);
|
|
}
|
|
|
|
// Use this for initialization
|
|
void Start () {
|
|
_chassisRigidBody = FindFirstParentRigidBody ();
|
|
UpdateRelativePosition ();
|
|
|
|
|
|
//_breakTorque = 10000;
|
|
float tire_mass = 4.0f;
|
|
float rim_mass = 15.0f;
|
|
float rim_radius = 0.331f;
|
|
float tire_radius = 0.556f;
|
|
|
|
float tire_inertia = tire_mass * tire_radius * tire_radius;
|
|
float rim_inertia = rim_mass * rim_radius * rim_radius;
|
|
SetInertia(tire_inertia + rim_inertia);
|
|
}
|
|
|
|
void SetInertia(float value)
|
|
{
|
|
_driveShaft.Inv_inertia = 1.0f / value;
|
|
}
|
|
|
|
private void UpdateSlipAngles(){
|
|
_localVelocity = transform.InverseTransformDirection (_velocity);
|
|
Quaternion q = Quaternion.AngleAxis (-SteerAngle, transform.up);
|
|
var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
|
|
_localVelocity = trs.MultiplyPoint3x4 (_localVelocity);
|
|
|
|
float rot_velocity = AngularVelocity * wheelRadius;
|
|
float lon_velocity = _localVelocity.z;
|
|
float lat_velocity = _localVelocity.x;
|
|
float lon_slip_velocity = lon_velocity - rot_velocity;
|
|
|
|
if (Mathf.Abs (lon_slip_velocity) <= 1.0e-4f)
|
|
{
|
|
_slip = Vector2.zero;
|
|
return;
|
|
}
|
|
|
|
float denom = Mathf.Max(Mathf.Abs(lon_velocity), 1e-0f);
|
|
_slip.x = -lon_slip_velocity / denom;
|
|
_slip.y = Mathf.Atan(lat_velocity / denom);
|
|
}
|
|
|
|
private void UpdateFriction(){
|
|
float surfaceFriction = 0.9f;
|
|
|
|
var normalForce = 4000.0f;// Mathf.Clamp(-_suspensionForce, 0, 10000);
|
|
if(normalForce * surfaceFriction < 0.00001f){
|
|
_peakSlip = Vector3.one;
|
|
_friction = Vector3.zero;
|
|
return;
|
|
}
|
|
|
|
//compute Pacejka params
|
|
var lateralParams = Pacejka.LateralParams(normalForce, camber, _coeffs);
|
|
var longitudinalParams = Pacejka.LongitudinalParams(normalForce, _coeffs);
|
|
var aligningParams = Pacejka.AligningParams(normalForce, camber, _coeffs);
|
|
|
|
_peakSlip.x = Pacejka.Extremum(longitudinalParams);
|
|
_peakSlip.y = Pacejka.Extremum(lateralParams);
|
|
_peakSlip.z = Pacejka.Extremum(aligningParams);
|
|
|
|
float epsilon = 0.000001f;
|
|
|
|
Vector3 slip = new Vector3();
|
|
if(Mathf.Abs(_peakSlip.x) >= epsilon){
|
|
slip.x = _slip.x / _peakSlip.x;
|
|
}
|
|
if(Mathf.Abs(_peakSlip.y) >= epsilon){
|
|
slip.y = _slip.y / _peakSlip.y;
|
|
}
|
|
if(Mathf.Abs(_peakSlip.z) >= epsilon){
|
|
slip.z = _slip.y / _peakSlip.z;
|
|
}
|
|
|
|
|
|
//Combined friction (friction circle)
|
|
float slip_xy = new Vector3 (slip.x, slip.y, 0.0f).magnitude;
|
|
float slip_xz = new Vector3 (slip.x, 0.0f, slip.z).magnitude;
|
|
|
|
|
|
if ((Mathf.Abs(normalForce) > epsilon) && (_localVelocity.magnitude > 0.001f)) {
|
|
_friction.x = Pacejka.MagicFormula(Mathf.Sign(slip.x) * slip_xy * _peakSlip.x, longitudinalParams) * surfaceFriction;
|
|
_friction.y = Pacejka.MagicFormula(Mathf.Sign(slip.y) * slip_xy * _peakSlip.y, lateralParams) * surfaceFriction;
|
|
_friction.z = Pacejka.MagicFormula(slip_xz * _peakSlip.z, aligningParams) * surfaceFriction;
|
|
|
|
if (slip_xy > epsilon)
|
|
{
|
|
_friction.x *= Mathf.Abs (slip.x) / slip_xy;
|
|
_friction.y *= Mathf.Abs (slip.y) / slip_xy;
|
|
}
|
|
if (slip_xz > epsilon)
|
|
_friction.z *= slip.z / slip_xz;
|
|
}else{
|
|
_friction = Vector3.zero;
|
|
}
|
|
}
|
|
|
|
public float AngularVelocity{
|
|
get{return _driveShaft.AngularVelocity;}
|
|
}
|
|
|
|
public void FixedUpdate(){
|
|
UpdateRelativePosition ();
|
|
|
|
|
|
Quaternion q = Quaternion.AngleAxis (SteerAngle, transform.up);
|
|
var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
|
|
var right = trs.MultiplyPoint3x4 (transform.right).normalized;
|
|
var forward = trs.MultiplyPoint3x4 (transform.forward).normalized;
|
|
|
|
//get wheel joint velocity
|
|
_velocity = _chassisRigidBody.GetPointVelocity (transform.position);
|
|
|
|
|
|
UpdateSlipAngles ();
|
|
UpdateFriction ();
|
|
|
|
|
|
*/
|
|
|
|
|
|
/*var raycastRay = new Ray (transform.position, -transform.up);
|
|
|
|
RaycastHit rayHit;
|
|
if (Physics.Raycast (raycastRay, out rayHit)) {
|
|
if(rayHit.distance <= (wheelRadius + suspensionDistance)){
|
|
_isGrounded = true;
|
|
currentLength = 1.0f - (rayHit.distance - wheelRadius) / suspensionDistance;
|
|
|
|
|
|
_suspensionForce = suspensionSpring * (targetPosition - currentLength) + suspensionDamper * (previousLength - currentLength) / Time.fixedDeltaTime;
|
|
//_chassisRigidBody.AddForceAtPosition(rayHit.normal * -_suspensionForce, transform.position);
|
|
|
|
|
|
previousLength = currentLength;
|
|
_hit.point = rayHit.point;
|
|
_hit.normal = rayHit.normal;
|
|
|
|
|
|
|
|
|
|
var friction = forward * _friction.x- right * _friction.y;
|
|
var frictionTorque = _friction.x * wheelRadius;
|
|
|
|
var reaction_torque = frictionTorque;
|
|
var applied_torque = _driveTorque + _breakTorque;
|
|
|
|
if ( ( applied_torque > 0 && reaction_torque > applied_torque ) ||
|
|
( applied_torque < 0 && reaction_torque < applied_torque ) )
|
|
reaction_torque = applied_torque;
|
|
|
|
var tire_torque = right * reaction_torque;
|
|
|
|
float matFriction = 0.8f;
|
|
float rolling = 0;// matFriction * (0.0136f + 0.4e-7f * AngularVelocity * AngularVelocity);
|
|
|
|
float rolling_resistance_torque = rolling * wheelRadius - frictionTorque;
|
|
float wheel_torque = _driveTorque + _breakTorque + rolling_resistance_torque;
|
|
|
|
|
|
this.SetTorque(wheel_torque, Time.fixedDeltaTime);
|
|
this.Integrate(Time.fixedDeltaTime);
|
|
|
|
|
|
|
|
_chassisRigidBody.AddForceAtPosition(friction, rayHit.point);
|
|
//_chassisRigidBody.AddForceAtPosition(forward * _friction.x, transform.position);
|
|
|
|
}else{
|
|
currentLength = suspensionDistance;
|
|
previousLength = currentLength;
|
|
_isGrounded = false;
|
|
_friction = Vector3.zero;
|
|
|
|
}
|
|
|
|
}
|
|
else{
|
|
currentLength = suspensionDistance;
|
|
previousLength = currentLength;
|
|
_isGrounded = false;
|
|
_friction = Vector3.zero;
|
|
}
|
|
|
|
_currentSuspensionPosition = currentLength;*/
|
|
/*}
|
|
|
|
private Rigidbody FindFirstParentRigidBody(){
|
|
var currentTransform = transform;
|
|
while (true) {
|
|
var parent = currentTransform.parent;
|
|
if(parent == null){
|
|
return null;
|
|
}
|
|
|
|
var rb = parent.gameObject.GetComponent<Rigidbody>();
|
|
if(rb != null){
|
|
return rb;
|
|
}
|
|
currentTransform = parent;
|
|
}
|
|
}
|
|
|
|
// Update is called once per frame
|
|
void Update () {
|
|
|
|
}
|
|
|
|
void OnGUI(){
|
|
int id = int.Parse (name [name.Length - 1].ToString ());
|
|
SteerAngle = GUI.HorizontalSlider (new Rect(10, 20 * id, 100,20), SteerAngle, -30, 30);
|
|
GUI.Label (new Rect (120, 20 * id, 100, 20), SteerAngle.ToString());
|
|
}
|
|
|
|
|
|
void OnDrawGizmosSelected(){
|
|
if (!DrawDebug) {
|
|
return;
|
|
}
|
|
var p = _coeffs;
|
|
|
|
Curve[] curves = new Curve[]{new Curve(), new Curve(), new Curve()};
|
|
|
|
int sampleCnt = 150;
|
|
float minArg = -20;
|
|
float maxArg = 20;
|
|
float argDiff = maxArg - minArg;
|
|
|
|
for (int i = 0; i < sampleCnt; ++i) {
|
|
var arg = minArg + argDiff / ((float)sampleCnt - 1) * (float)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));
|
|
}
|
|
|
|
|
|
Tools.BeginGui ();
|
|
Plot2D plot = new Plot2D(curves[1]);
|
|
|
|
//Slip.peak_slip(
|
|
|
|
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 ();
|
|
}*/
|
|
|
|
//void OnDrawGizmos(){
|
|
|
|
//_suspensionForce = 5000.0f;
|
|
//UpdateSlipAngles ();
|
|
//UpdateFriction ();
|
|
|
|
|
|
/*if (suspensionDistance < 0.01f) {
|
|
suspensionDistance = 0.01f;
|
|
}*/
|
|
/*if (wheelRadius < 0.01f) {
|
|
wheelRadius = 0.01f;
|
|
}
|
|
//targetPosition = Mathf.Clamp01 (targetPosition);
|
|
|
|
Vector3 wheelPosition;
|
|
Vector3 bottom;
|
|
*/
|
|
/*if (Application.isPlaying) {
|
|
wheelPosition = transform.position - transform.up.normalized * (1 - _currentSuspensionPosition) * suspensionDistance;
|
|
|
|
}else{
|
|
wheelPosition = transform.position;
|
|
}*/
|
|
|
|
|
|
//bottom = transform.position - transform.up.normalized * suspensionDistance;
|
|
|
|
/*float kSuspW = 0.1f;
|
|
float suspHW = wheelRadius * kSuspW * 0.5f;
|
|
|
|
Quaternion q = Quaternion.AngleAxis (SteerAngle, transform.up);
|
|
var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
|
|
var right = trs.MultiplyPoint3x4 (transform.right);
|
|
var forward = trs.MultiplyPoint3x4 (transform.forward);
|
|
*/
|
|
|
|
/*Gizmos.color = Color.red;
|
|
//draw suspension
|
|
Gizmos.DrawLine (transform.position, bottom);
|
|
Gizmos.DrawLine (transform.position - forward * suspHW * 1.5f, transform.position + forward * suspHW* 1.5f);
|
|
Gizmos.DrawLine (bottom - forward * suspHW, bottom + forward * suspHW);
|
|
*/
|
|
|
|
|
|
/*var colliderColor = new Color (0, 1, 0, 0.5f);
|
|
Gizmos.color = colliderColor;
|
|
|
|
|
|
//draw wheel radius
|
|
var wheelCenter = transform.position;
|
|
var rollMatrix = Matrix4x4.TRS (Vector3.zero, Quaternion.AngleAxis(_driveShaft.Angle * Mathf.Rad2Deg, right), Vector3.one);
|
|
var radiusP0 = -forward * wheelRadius;
|
|
var radiusP1 = forward * wheelRadius * 1.2f;
|
|
radiusP0 = rollMatrix.MultiplyPoint3x4 (radiusP0);
|
|
radiusP1 = rollMatrix.MultiplyPoint3x4 (radiusP1);
|
|
Gizmos.DrawLine (wheelCenter + radiusP0, wheelCenter + radiusP1);
|
|
|
|
*/
|
|
|
|
|
|
//draw wheel circle
|
|
//AdvancedGizmo.DrawCircle (wheelCenter, right, wheelRadius, colliderColor, 36);
|
|
|
|
//draw force point
|
|
/*var wheelBottom = wheelCenter - transform.up * wheelRadius;
|
|
AdvancedGizmo.DrawCircle (wheelBottom, forward, wheelRadius * kSuspW, colliderColor);
|
|
AdvancedGizmo.DrawCircle (wheelBottom, right, wheelRadius * kSuspW, colliderColor);
|
|
AdvancedGizmo.DrawCircle (wheelBottom, transform.up, wheelRadius * kSuspW, colliderColor);
|
|
*/
|
|
//draw friction
|
|
/*Gizmos.color = Color.red;
|
|
Gizmos.DrawLine (transform.position, transform.position + forward * _friction.x);
|
|
|
|
//draw wheel velocity
|
|
Gizmos.color = colliderColor;
|
|
Gizmos.DrawLine (transform.position, transform.position + _velocity);
|
|
|
|
var p = Camera.current.WorldToScreenPoint (transform.position + Vector3.up);
|
|
p.y = Camera.current.pixelHeight - p.y;
|
|
|
|
|
|
|
|
|
|
Tools.BeginGui ();
|
|
var labelRect = new Rect (p.x, p.y, 90, 20);
|
|
Tools.DrawRectangle (labelRect, new Color(0,0,0,0.5f));
|
|
GUI.Label (new Rect (p.x, p.y, 100, 100), _friction.x.ToString ());
|
|
|
|
Tools.EndGui ();
|
|
}*/
|
|
//}
|