*

Unity/Assets/Scripts/Car/Suspension.cs

@@ -0,0 +1,142 @@
+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 () {
+		/*   */
+	}
+}