Use SG_LOG for debugging messages from the YASim helicopter model.

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

diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp
index 056bc61a470909e3b8809ba16bfccb2dee1500a2..51492dcfac9d98dcd6b7cfc473323e7fc521020e 100644 (file)
--- a/src/FDM/YASim/Gear.cpp
+++ b/src/FDM/YASim/Gear.cpp
@@ -6,25 +6,29 @@ namespace yasim {
 
 Gear::Gear()
 {
-    for(int i=0; i<3; i++)
+    int i;
+    for(i=0; i<3; i++)
        _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;
 }
 
 void Gear::setPosition(float* position)
 {
-    for(int i=0; i<3; i++) _pos[i] = position[i];
+    int i;
+    for(i=0; i<3; i++) _pos[i] = position[i];
 }
 
 void Gear::setCompression(float* compression)
 {
-    for(int i=0; i<3; i++) _cmpr[i] = compression[i];
+    int i;
+    for(i=0; i<3; i++) _cmpr[i] = compression[i];
 }
 
 void Gear::setSpring(float spring)
@@ -49,7 +53,7 @@ void Gear::setDynamicFriction(float dfric)
 
 void Gear::setBrake(float brake)
 {
-    _brake = brake;
+    _brake = Math::clamp(brake, 0, 1);
 }
 
 void Gear::setRotation(float rotation)
@@ -59,17 +63,24 @@ void Gear::setRotation(float rotation)
 
 void Gear::setExtension(float extension)
 {
-    _extension = extension;
+    _extension = Math::clamp(extension, 0, 1);
+}
+
+void Gear::setCastering(bool c)
+{
+    _castering = c;
 }
 
 void Gear::getPosition(float* out)
 {
-    for(int i=0; i<3; i++) out[i] = _pos[i];
+    int i;
+    for(i=0; i<3; i++) out[i] = _pos[i];
 }
 
 void Gear::getCompression(float* out)
 {
-    for(int i=0; i<3; i++) out[i] = _cmpr[i];    
+    int i;
+    for(i=0; i<3; i++) out[i] = _cmpr[i];    
 }
 
 float Gear::getSpring()
@@ -123,10 +134,16 @@ float Gear::getCompressFraction()
     return _frac;
 }
 
+bool Gear::getCastering()
+{
+    return _castering;
+}
+
 void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
 {
     // Init the return values
-    for(int i=0; i<3; i++) _force[i] = _contact[i] = 0;
+    int i;
+    for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
 
     // Don't bother if it's not down
     if(_extension < 1)
@@ -137,8 +154,11 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     // 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);
-    if(a > 0)
+    if(a > 0) {
+       _wow = 0;
+       _frac = 0;
        return;
+    }
 
     // Now a is the distance from the tip to ground, so make b the
     // distance from the base to ground.  We can get the fraction
@@ -150,7 +170,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     // Calculate the point of ground _contact.
     _frac = a/(a-b);
     if(b < 0) _frac = 1;
-    for(int i=0; i<3; i++)
+    for(i=0; i<3; i++)
        _contact[i] = _pos[i] + _frac*_cmpr[i];
 
     // Turn _cmpr into a unit vector and a magnitude
@@ -163,11 +183,10 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     body->pointVelocity(_contact, rot, cv);
     Math::add3(cv, v, cv);
 
-    // Finally, we can start adding up the forces.  First the
-    // spring compression (note the clamping of _frac to 1):
+    // Finally, we can start adding up the forces.  First the spring
+    // compression.   (note the clamping of _frac to 1):
     _frac = (_frac > 1) ? 1 : _frac;
     float fmag = _frac*clen*_spring;
-    Math::mul3(fmag, cmpr, _force);
 
     // Then the damping.  Use the only the velocity into the ground
     // (projection along "ground") projected along the compression
@@ -177,10 +196,15 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     float damp = _damp * dv;
     if(damp > fmag) damp = fmag; // can't pull the plane down!
     if(damp < -fmag) damp = -fmag; // sanity
-    Math::mul3(-damp, cmpr, tmp);
-    Math::add3(_force, tmp, _force);
 
-    _wow = fmag - damp;    
+    // The actual force applied is only the component perpendicular to
+    // the ground.  Side forces come from velocity only.
+    _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,
@@ -228,9 +252,9 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
 
 float Gear::calcFriction(float wgt, float v)
 {
-    // How slow is stopped?  50 cm/second?
-    const float STOP = 0.5;
-    const float iSTOP = 1/STOP;
+    // How slow is stopped?  10 cm/second?
+    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;