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