143 lines
4.1 KiB
C#
143 lines
4.1 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
|
|
public abstract class Suspension : MonoBehaviour {
|
|
|
|
private double _steer;
|
|
protected double _normalForce;
|
|
public WheelPhysics wheel;
|
|
public double travelDistance = 0.2f;
|
|
public double suspensionTarget = 0.2f;
|
|
private double _displacement;
|
|
protected double _previousFraction;
|
|
private double _overtravel;
|
|
private double _lastDisplacement;
|
|
public double springForce = 9000;
|
|
public double springDamping = 400;
|
|
|
|
public double Steer{
|
|
get{return _steer;}
|
|
set
|
|
{
|
|
_steer = value;
|
|
OnSteerChanged();
|
|
}
|
|
}
|
|
|
|
public virtual void OnSteerChanged() {
|
|
|
|
}
|
|
|
|
// Use this for initialization
|
|
void Start () {
|
|
|
|
}
|
|
|
|
public double Displacement {
|
|
get{return _displacement;}
|
|
set{
|
|
_lastDisplacement = _displacement;
|
|
_displacement = MathEx.Clamp(value, 0, travelDistance);
|
|
_overtravel = value - travelDistance;
|
|
if(_overtravel < 0){
|
|
_overtravel = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void CalculateForce(double dt){
|
|
double velocity = ((_displacement - _lastDisplacement) / travelDistance) / dt;
|
|
double spring = (_displacement / travelDistance) * springForce;
|
|
double damping = velocity * springDamping;
|
|
_normalForce = damping + spring;
|
|
}
|
|
|
|
|
|
double computeImpulseDenominator(Vector3 pos, Vector3 normal, Rigidbody rb){
|
|
Vector3 r0 = pos - rb.worldCenterOfMass;
|
|
Vector3 c0 = Vector3.Cross(r0, normal);
|
|
|
|
|
|
var tensorLocal = Matrix4x4.TRS(rb.worldCenterOfMass, rb.inertiaTensorRotation, rb.inertiaTensor);
|
|
var tensorWorld = transform.localToWorldMatrix * tensorLocal;
|
|
tensorWorld = Matrix4x4.Inverse(tensorWorld);
|
|
Vector3 vec = Vector3.Cross(tensorWorld.MultiplyPoint(c0), r0);
|
|
|
|
return 1.0f / rb.mass + Vector3.Dot(normal, vec);
|
|
}
|
|
|
|
protected Vector3 dbgWheelForce;
|
|
protected Vector3 dbgWheelForcePos;
|
|
|
|
public void Integrate(double dt, Car car){
|
|
if(wheel != null){
|
|
//var rb = car.GetRigidBody();
|
|
|
|
//raycast wheel
|
|
wheel.UpdateContactPoint(car.transform.up);
|
|
double fraction;
|
|
double relativeDisplacement = wheel.TotalWheelRadius - wheel.Contact.hit.distance;
|
|
Displacement += relativeDisplacement;
|
|
|
|
fraction = _displacement / travelDistance;
|
|
CalculateForce(dt);
|
|
|
|
var wheelWorldVelocity = car.GetRigidBody().GetPointVelocity(wheel.transform.position);
|
|
|
|
if (_overtravel > 0)
|
|
{
|
|
double correction_factor = 0.0f;
|
|
double dv = Vector3.Dot(wheelWorldVelocity, transform.up);// body->getVelocityInLocalPoint(suspension_force_application_point).dot(forcedirection);
|
|
dv -= correction_factor * _overtravel / dt;
|
|
// double effectiveMass = 1.0 / computeImpulseDenominator(wheel.transform.position, transform.up, car.GetRigidBody());//1.0 / body->computeImpulseDenominator(wheel_position[i], forcedirection);
|
|
// double correction = -effectiveMass * dv / dt;
|
|
/*if (correction > 0)
|
|
{
|
|
_normalForce = correction;
|
|
}*/
|
|
}
|
|
|
|
if(!wheel.Contact.hasHit) {
|
|
_normalForce = 0;
|
|
}
|
|
|
|
if(_normalForce > 0.0f){
|
|
car.GetRigidBody().AddForceAtPosition(wheel.Contact.hit.normal * (float)_normalForce, transform.position);
|
|
}
|
|
|
|
wheel.transform.position = GetWheelPosition(fraction);
|
|
|
|
Vector3 wheelForce;
|
|
wheel.Integrate(dt, wheelWorldVelocity, _normalForce, out wheelForce);
|
|
car.GetRigidBody().AddForceAtPosition(wheelForce, wheel.transform.position);
|
|
|
|
dbgWheelForce = wheelForce;
|
|
dbgWheelForcePos = wheel.transform.position;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
public void CalculateSuspensionForce(double dt, double fraction) {
|
|
_normalForce = springForce * (suspensionTarget - fraction) + springDamping * (_previousFraction - fraction) / dt;
|
|
_previousFraction = fraction;
|
|
}
|
|
|
|
public Vector3 GetWheelPosition(double fraction) {
|
|
var up = transform.up;
|
|
var center = transform.position;
|
|
var top = center + up * (float)((1.0 - suspensionTarget) * travelDistance);
|
|
var bottom = center - up * (float)(suspensionTarget * travelDistance);
|
|
|
|
return bottom + (top - bottom) * Mathf.Clamp01((float)fraction);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Update is called once per frame
|
|
void Update () {
|
|
/* */
|
|
}
|
|
}
|