Use bool where the source and destination variable is bool.

[flightgear.git] / src / FDM / YASim / Gear.cpp

diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp
index e50c4d000ece75384fadcb2675ea57b1e4fc155f..1936c3594bc4016240908396559a8c5565504a7f 100644 (file)
--- a/src/FDM/YASim/Gear.cpp
+++ b/src/FDM/YASim/Gear.cpp
@@ -1,4 +1,5 @@
 #include "Math.hpp"
+#include "BodyEnvironment.hpp"
 #include "RigidBody.hpp"
 
 #include "Gear.hpp"
@@ -11,12 +12,18 @@ Gear::Gear()
        _pos[i] = _cmpr[i] = 0;
     _spring = 1;
     _damp = 0;
-    _sfric = 0.8;
-    _dfric = 0.7;
+    _sfric = 0.8f;
+    _dfric = 0.7f;
     _brake = 0;
     _rot = 0;
     _extension = 1;
     _castering = false;
+    _frac = 0;
+
+    for(i=0; i<3; i++)
+       _global_ground[i] = _global_vel[i] = 0;
+    _global_ground[2] = 1;
+    _global_ground[3] = -1e3;
 }
 
 void Gear::setPosition(float* position)
@@ -71,6 +78,13 @@ void Gear::setCastering(bool c)
     _castering = c;
 }
 
+void Gear::setGlobalGround(double *global_ground, float* global_vel)
+{
+    int i;
+    for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
+    for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
+}
+
 void Gear::getPosition(float* out)
 {
     int i;
@@ -83,6 +97,12 @@ void Gear::getCompression(float* out)
     for(i=0; i<3; i++) out[i] = _cmpr[i];    
 }
 
+void Gear::getGlobalGround(double* global_ground)
+{
+    int i;
+    for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
+}
+
 float Gear::getSpring()
 {
     return _spring;
@@ -139,7 +159,7 @@ bool Gear::getCastering()
     return _castering;
 }
 
-void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
+void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
 {
     // Init the return values
     int i;
@@ -149,14 +169,24 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     if(_extension < 1)
        return;
 
-    float tmp[3];
+    // The ground plane transformed to the local frame.
+    float ground[4];
+    s->planeGlobalToLocal(_global_ground, ground);
+        
+    // The velocity of the contact patch transformed to local coordinates.
+    float glvel[3];
+    s->velGlobalToLocal(_global_vel, glvel);
 
     // First off, make sure that the gear "tip" is below the ground.
     // If it's not, there's no force.
     float a = ground[3] - Math::dot3(_pos, ground);
+    _compressDist = -a;
     if(a > 0) {
        _wow = 0;
        _frac = 0;
+        _compressDist = 0;
+        _rollSpeed = 0;
+        _casterAngle = 0;
        return;
     }
 
@@ -164,6 +194,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     // distance from the base to ground.  We can get the fraction
     // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
     // above ground is negative.
+    float tmp[3];
     Math::add3(_cmpr, _pos, tmp);
     float b = ground[3] - Math::dot3(tmp, ground);
 
@@ -182,6 +213,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     float cv[3];
     body->pointVelocity(_contact, rot, cv);
     Math::add3(cv, v, cv);
+    Math::sub3(cv, glvel, cv);
 
     // Finally, we can start adding up the forces.  First the spring
     // compression.   (note the clamping of _frac to 1):
@@ -202,10 +234,6 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
     Math::mul3(-_wow, ground, _force);
 
-    // Castering gear feel no force in the ground plane
-    if(_castering)
-       return;
-
     // Wheels are funky.  Split the velocity along the ground plane
     // into rolling and skidding components.  Assuming small angles,
     // we generate "forward" and "left" unit vectors (the compression
@@ -226,17 +254,37 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     Math::cross3(skid, gup, steer); // skid cross up == steer
 
     if(_rot != 0) {
-       // Correct for a (small) rotation
-       Math::mul3(_rot, steer, tmp);
-       Math::add3(tmp, skid, skid);
-       Math::unit3(skid, skid);
-       Math::cross3(skid, gup, steer);
+       // Correct for a rotation
+        float srot = Math::sin(_rot);
+        float crot = Math::cos(_rot);
+        float tx = steer[0];
+        float ty = steer[1];
+        steer[0] =  crot*tx + srot*ty;
+        steer[1] = -srot*tx + crot*ty;
+
+        tx = skid[0];
+        ty = skid[1];
+        skid[0] =  crot*tx + srot*ty;
+        skid[1] = -srot*tx + crot*ty;
     }
 
     float vsteer = Math::dot3(cv, steer);
     float vskid  = Math::dot3(cv, skid);
     float wgt = Math::dot3(_force, gup); // force into the ground
 
+    if(_castering) {
+        _rollSpeed = Math::sqrt(vsteer*vsteer + vskid*vskid);
+        // Don't modify caster angle when the wheel isn't moving,
+        // or else the angle will animate the "jitter" of a stopped
+        // gear.
+        if(_rollSpeed > 0.05)
+            _casterAngle = Math::atan2(vskid, vsteer);
+        return;
+    } else {
+        _rollSpeed = vsteer;
+        _casterAngle = _rot;
+    }
+
     float fsteer = _brake * calcFriction(wgt, vsteer);
     float fskid  = calcFriction(wgt, vskid);
     if(vsteer > 0) fsteer = -fsteer;
@@ -253,8 +301,8 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
 float Gear::calcFriction(float wgt, float v)
 {
     // How slow is stopped?  10 cm/second?
-    const float STOP = 0.1;
-    const float iSTOP = 1/STOP;
+    const float STOP = 0.1f;
+    const float iSTOP = 1.0f/STOP;
     v = Math::abs(v);
     if(v < STOP) return v*iSTOP * wgt * _sfric;
     else         return wgt * _dfric;