src/FDM/YASim/: added support for torus-shaped gear contact surface.

Contact surface can now be a torus shaped tyre; we find the part of this torus
that is furthest towards the ground instead of using a fixed contact point.

Torus is specified by radius of wheel centred on original contact point,
orientation of a wheel axle, and radius of torus. If both wheel and tyre radius
are both zero the calculations reduce to a fixed contact point as before.

src/FDM/YASim/FGFDM.cpp
    Read new optional <gear> attributes:
        wheel-axle-x
        wheel-axle-y
        wheel-axle-z
        wheel-radius
        tyre-radius

src/FDM/YASim/Gear.cpp
    Added new gearCompression() function that calculates gear compression for
    wheel with torus tyre. If new radius values are both zero the calculation
    behaves like a contact point as before; we also optimse this case to avoid
    the extra matrix operations.

    Moved old algorithm into gearCompressionOld() function to allow testing
    that new algorithm gives same results when wheel and tyre have zero radius.

src/FDM/YASim/Gear.hpp
    Added new members:
        _wheelAxle,
        _wheelRadius
        _tyreRadius
    Also declare gearCompression() function so it is available for unit
    testing.

    Added GearVector struct for caching unit vector and magnitude and use for
    _compr and _wheelAxle, so we avoid having to calculate magnitude and unit
    vector each iteration.

    Clarified that _compressDist is vertical movement of gear. Not useful for
    gear animations - one must use compression-norm.

src/FDM/YASim/Airplane.cpp
src/FDM/YASim/Model.cpp
src/FDM/YASim/YASim.cxx
    Use g->getContact() instead of manually adding compression vector to
    g->getPosition() (which no longer gives the correct contact point).

src/FDM/YASim/Airplane.cpp

@@ -442,6 +442,7 @@ void Airplane::compileGear(GearRec* gr)
     g->getCompression(cmp);
     float length = 3 * Math::mag3(cmp);
     g->getPosition(pos);
+    pos[2] -= (g->getWheelRadius() + g->getTyreRadius());
     Math::mul3(0.5, cmp, cmp);
     Math::add3(pos, cmp, pos);
 
@@ -652,6 +653,10 @@ void Airplane::solveGear()
         GearRec* gr = (GearRec*)_gears.get(i);
         Gear* g = gr->gear;
         g->getPosition(pos);
+        // Move pos to bottom of wheel if specified; in this case the exact
+        // contact point depends on aircraft orientation relative to ground,
+        // but this is probably good enough here.
+        pos[2] -= (g->getWheelRadius() + g->getTyreRadius());
         Math::sub3(cg, pos, pos);
         gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
         if (!g->getIgnoreWhileSolving())

src/FDM/YASim/FGFDM.cpp

@@ -6,6 +6,8 @@
 #include <stdlib.h>
 #include <cstring>
 
+#include <simgear/debug/logstream.hxx>
+
 #include <Main/fg_props.hxx>
 
 #include "yasim-common.hpp"
@@ -1057,6 +1059,22 @@ void FGFDM::parseGear(const XMLAttributes* a)
     for(int i=0; i<3; i++)
         v[i] *= attrf(a, "compression", 1);
     g->setCompression(v);
+    
+    if (a->hasAttribute("wheel-axle-x")) {
+        v[0] = attrf(a, "wheel-axle-x");
+        v[1] = attrf(a, "wheel-axle-y");
+        v[2] = attrf(a, "wheel-axle-z");
+    }
+    else {
+        v[0] = 0;
+        v[1] = 1;
+        v[2] = 0;
+    }
+    g->setWheelAxle( v);
+    
+    g->setWheelRadius( attrf(a, "wheel-radius", 0));
+    g->setTyreRadius( attrf(a, "tyre-radius", 0));
+    
     g->setBrake(attrf(a, "skid", 0));
     g->setInitialLoad(attrf(a, "initial-load", 0));
     g->setStaticFriction(attrf(a, "sfric", 0.8));

src/FDM/YASim/Gear.cpp

@@ -3,6 +3,8 @@
 #  include "config.h"
 #endif
 
+#include <simgear/debug/logstream.hxx>
+
 #include "Math.hpp"
 #include "BodyEnvironment.hpp"
 #include "Ground.hpp"
@@ -18,11 +20,38 @@ static const float DEG2RAD = YASIM_PI / 180.0;
 static const float maxGroundBumpAmplitude=0.4;
         //Amplitude can be positive and negative
 
+/* Sets <out> to unit vector along <v>, or all zeros if <v> is zero
+length. Returns length of <v>. */
+static float magnitudeUnit( const float* v, float* out)
+{
+    float magnitude = Math::mag3( v);
+    if ( magnitude) {
+        Math::mul3( 1 / magnitude, v, out);
+    }
+    else {
+        Math::zero3( out);
+    }
+    return magnitude;
+}
+
+GearVector::GearVector( float v0, float v1, float v2)
+{
+    set( v0, v1, v2);
+}
+
+void GearVector::set( float v0, float v1, float v2)
+{
+    v[0] = v0;
+    v[1] = v1;
+    v[2] = v2;
+    magnitude = magnitudeUnit( v, unit);
+}
+
 Gear::Gear()
 {
     int i;
     for(i=0; i<3; i++)
-        _pos[i] = _cmpr[i] = _stuck[i] = 0;
+        _pos[i] = _stuck[i] = 0;
     _spring = 1;
     _damp = 0;
     _sfric = 0.8f;
@@ -59,6 +88,23 @@ Gear::Gear()
 
     Math::zero3(_ground_trans);
     Math::identity33(_ground_rot);
+    
+    _wheelAxle.set(0, 1, 0);
+    _wheelRadius = 0;
+    _tyreRadius = 0;
+}
+
+void Gear::setCompression(float* compression)
+{
+    _cmpr.set( compression[0], compression[1], compression[2]);
+}
+
+void Gear::setWheelAxle(float* wheel_axle)
+{
+    _wheelAxle.set( wheel_axle[0], wheel_axle[1], wheel_axle[2]);
+    if (_wheelAxle.magnitude == 0) {
+        _wheelAxle.unit[1] = 1;
+    }
 }
 
 void Gear::setGlobalGround(double *global_ground, float* global_vel,
@@ -152,6 +198,206 @@ void Gear::integrate(float dt)
     return;
 }
 
+
+bool gearCompression(
+        const float (&ground)[4],
+        const GearVector& compression,
+        const float (&wheel_pos)[3],
+        const GearVector (&wheel_axle),
+        float wheel_radius,
+        float tyre_radius,
+        std::function< float ()> bump_fn,
+        float (&o_contact)[3],
+        float& o_compression_distance_vertical,
+        float& o_compression_norm
+        )
+{
+    /* In this diagram we are looking at wheel end on.
+
+                N
+                 \     S'
+                  \   /
+                   \ /
+                    W--__
+                   /     --> wheel_axle     |
+                  /                         | G
+                 S                          V
+
+        ------------------------- Ground plus tyre_radius
+
+        ========================= Ground
+
+    W is wheel_pos, position of wheel centre at maximum extension.
+
+    S is the lowest point of rim relative to ground. S' is the highest point
+    of the rim relative to the ground. So SWS' is a diameter across middle of
+    wheel. For most aircraft this will be vertical (relative to the aircraft),
+    but we don't require this; might be useful for a Bf109 aircraft.
+
+    N is position of wheel centre at maximum compression:
+        N = W + compression.
+
+    <wheel_axle> is wheel axle vector. Usually this is close to y axis, e.g. (
+    0, 1, 0).
+
+    G is ground normal, pointing downwards. Points p on ground satisfy:
+        G[:3].p = G3
+
+    Points p on (ground plus tyre_radius) satisfy:
+        G[:3].p = G3 + tyre_radius.
+
+    R is vector from W to S; to make S be lowest point on rim, this is radial
+    vector of wheel that is most closely aligned with G. We use:
+        R = - wheel_axle x G x wheel_axle
+
+    Distance of S below (ground plus tyre_radius), is:
+        a = S.G - ( G3 + tyre_radius)
+
+    If a < 0, the point of maximum extension is above ground and we return
+    false.
+
+    Otherwise, a >= 0 and the point of maximum extension is below ground, so
+    we are in contact with the ground and we compress the grear just enough to
+    make S be on ground plus tyre_radius. The tyre surface is tyre_radius from
+    S, so this ensures that the tyre will just touch the ground.
+
+    Compressing the gear means we move S (and W and S') a distance
+    compression_distance along vector <compression> (the vector from W to N),
+    and the projection of this movement along G must be -a. So:
+        ( compression_distance * compression) . G = -a
+    So:
+        compression_distance = -a / (compression . G)
+    */
+    
+    float ground_unit[3];
+    magnitudeUnit( ground, ground_unit);
+    
+    /* Find S, the lowest point on wheel. */
+    float S[3];
+    Math::set3( wheel_pos, S);
+    
+    if (wheel_radius) {
+        /* Find radial wheel vector pointing closest to ground using two
+        cross-products: wheel_axle_unit x ground_unit x wheel_axle_unit */
+        float R[3];
+        Math::cross3( wheel_axle.unit, ground_unit, R);
+        Math::cross3( R, wheel_axle.unit, R);
+
+        /* Scale R to be length wheel_radius so it is vector from W to S, and
+        use it to find S. */
+        Math::unit3( R, R);
+        Math::mul3( wheel_radius, R, R);
+        
+        /* Add R to S to get lowest point on wheel. */
+        Math::add3( S, R, S);
+    }
+    
+    /* Calculate <a>, distance of S below ground. */
+    float a = Math::dot3( ground, S) - ( ground[3] - tyre_radius);
+    float bump_altitude = 0;
+    if ( a > -maxGroundBumpAmplitude) {
+        bump_altitude = bump_fn();
+        a -= bump_altitude;
+    }
+    
+    if ( a < 0) {
+        /* S is above ground so we are not on ground. */
+        return false;
+    }
+    
+    /* Lowest part of wheel is below ground. */
+    o_compression_distance_vertical = a;
+    
+    /* Find compression_norm. First we need to find compression_distance, the
+    distance to compress the gear so that S (which is below ground+tyre_radius)
+    would move to just touch ground+tyre_radius. We need to move gear further
+    depending on how perpendicular <compression> is to the ground. */
+    o_compression_norm = 0;
+    float compression_distance = 0;
+    float div = Math::dot3( compression.unit, ground_unit);
+    if (div) {
+        /* compression.unit points upwards, ground_unit points downwards, so
+        div is -ve. */
+        compression_distance = -a / div;
+    }
+
+    /* Set o_compression_norm. */
+    if ( compression.magnitude) {
+        o_compression_norm = compression_distance / compression.magnitude;
+    }
+    if (o_compression_norm > 1) {
+        o_compression_norm = 1;
+    }
+    
+    /* Contact point on ground-plus-tyre-radius is S plus compression
+    vector. */
+    float delta[3];
+    Math::mul3( compression_distance, compression.unit, delta);
+    Math::add3( S, delta, o_contact);
+
+    /* Check that <o_contact> is tyre_radius above ground. */
+    assert( fabs( Math::dot3( o_contact, ground) - (ground[3] - tyre_radius + bump_altitude)) < 0.001);
+
+    /* Correct for tyre_radius - need to move o_contact a distance tyre_radius
+    towards ground. */
+    if (tyre_radius) {
+        Math::mul3( tyre_radius, ground_unit, delta);
+        Math::add3( o_contact, delta, o_contact);
+    }
+    
+    /* Check that <o_contact> is on ground. */
+    assert( fabs( Math::dot3( o_contact, ground) - (ground[3] + bump_altitude)) < 0.001);
+    
+    return true;
+}
+
+bool gearCompressionOld(
+        const float (&ground)[4],
+        const GearVector& compression,
+        const float (&pos)[3],
+        std::function< float ()> bump_fn,
+        float (&o_contact)[3],
+        float& o_compression_distance_vertical,
+        float& o_compression_norm
+        )
+{
+    // 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);
+    float BumpAltitude=0;
+    if (a<maxGroundBumpAmplitude)
+    {
+        BumpAltitude = bump_fn();
+        a+=BumpAltitude;
+    }
+    
+    if(a > 0) {
+        o_compression_distance_vertical = 0;
+        o_compression_norm = 0;
+        return false;
+    }
+
+    o_compression_distance_vertical = -a;
+    
+    // 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
+    // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
+    // above ground is negative.
+    float tmp[3];
+    Math::add3(compression.v, pos, tmp);
+    float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
+
+    // Calculate the point of ground _contact.
+    if(b < 0)
+        o_compression_norm = 1;
+    else
+        o_compression_norm = a/(a-b);
+    for(int i=0; i<3; i++)
+        o_contact[i] = pos[i] + o_compression_norm * compression.v[i];
+    return true;
+}
+
+
 void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* rot)
 {
     // Init the return values
@@ -180,21 +426,19 @@ void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* r
     float ground[4];
     s->planeGlobalToLocal(_global_ground, ground);
 
-    // The velocity of the contact patch transformed to local coordinates.
+    bool on_ground = gearCompression(
-    float glvel[3];
+            ground,
-    s->globalToLocal(_global_vel, glvel);
+            _cmpr,
-
+            _pos,
-    // First off, make sure that the gear "tip" is below the ground.
+            _wheelAxle,
-    // If it's not, there's no force.
+            _wheelRadius,
-    float a = ground[3] - Math::dot3(_pos, ground);
+            _tyreRadius,
-    float BumpAltitude=0;
+            [this] { return this->getBumpAltitude(); },
-    if (a<maxGroundBumpAmplitude)
+            _contact,
-    {
+            _compressDist,
-        BumpAltitude=getBumpAltitude();
+            _frac
-        a+=BumpAltitude;
+            );
-    }
+    if (!on_ground) {
-    _compressDist = -a;
-    if(a > 0) {
         _wow = 0;
         _frac = 0;
         _compressDist = 0;
@@ -202,26 +446,13 @@ void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* r
         return;
     }
 
-    // Now a is the distance from the tip to ground, so make b the
+    // The velocity of the contact patch transformed to local coordinates.
-    // distance from the base to ground.  We can get the fraction
+    float glvel[3];
-    // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
+    s->globalToLocal(_global_vel, glvel);
-    // above ground is negative.
-    float tmp[3];
-    Math::add3(_cmpr, _pos, tmp);
-    float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
-
-    // Calculate the point of ground _contact.
-    if(b < 0)
-        _frac = 1;
-    else
-        _frac = a/(a-b);
-    for(i=0; i<3; i++)
-        _contact[i] = _pos[i] + _frac*_cmpr[i];
     
     // Turn _cmpr into a unit vector and a magnitude
-    float cmpr[3];
+    const float (&cmpr)[3] = _cmpr.unit;
-    float clen = Math::mag3(_cmpr);
+    const float &clen = _cmpr.magnitude;
-    Math::mul3(1/clen, _cmpr, cmpr);
 
     // Now get the velocity of the point of contact
     float cv[3];
@@ -254,6 +485,7 @@ void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* r
     // Then the damping.  Use the only the velocity into the ground
     // (projection along "ground") projected along the compression
     // axis.  So Vdamp = ground*(ground dot cv) dot cmpr
+    float tmp[3];
     Math::mul3(Math::dot3(ground, cv), ground, tmp);
     float dv = Math::dot3(cmpr, tmp);
     float damp = _damp * dv;

src/FDM/YASim/Gear.hpp

@@ -2,6 +2,9 @@
 #define _GEAR_HPP
 #include "Math.hpp"
 
+#include <functional>
+
+
 namespace simgear {
 class BVHMaterial;
 }
@@ -12,6 +15,17 @@ class Ground;
 class RigidBody;
 struct State;
 
+
+/* Contains a vector and pre-generates unit and magnitude values. */
+struct GearVector
+{
+    GearVector( float v0=0, float v1=0, float v2=0);
+    void set( float v0, float v1, float v2);
+    float v[3];
+    float unit[3];
+    float magnitude;
+};
+
 // A landing gear has the following parameters:
 //
 // position: a point in the aircraft's local coordinate system where the
@@ -37,8 +51,20 @@ public:
     // Externally set values
     void setPosition(float* position) { Math::set3(position, _pos); }
     void getPosition(float* out) { Math::set3(_pos, out); }
-    void setCompression(float* compression) { Math::set3(compression, _cmpr); }
+    void setCompression(float* compression);
-    void getCompression(float* out) { Math::set3(_cmpr, out); }
+    void getCompression(float* out) { Math::set3(_cmpr.v, out); }
+    
+    void setWheelAxle(float* wheelAxle);
+    void getWheelAxle(float* out) { Math::set3(_wheelAxle.v, out); }
+    
+    void setWheelRadius(float wheelRadius) { _wheelRadius = wheelRadius; }
+    float getWheelRadius() { return _wheelRadius; }
+    
+    void setTyreRadius(float tyreRadius) { _tyreRadius = tyreRadius; }
+    float getTyreRadius() { return _tyreRadius; }
+    
+    void getContact(float* contact) { Math::set3( _contact, contact); }
+    
     void setSpring(float spring) { _spring = spring; }
     float getSpring() { return _spring; }
     void setDamping(float damping) { _damp = damping; }
@@ -114,7 +140,7 @@ private:
     bool _slipping;
     float _pos[3];
     double _stuck[3];
-    float _cmpr[3];
+    GearVector _cmpr;
     float _spring;
     float _damp;
     float _sfric;
@@ -125,9 +151,9 @@ private:
     float _force[3];
     float _contact[3];
     float _wow;
-    float _frac;
+    float _frac;    /* Compression as fraction of maximum. */
     float _initialLoad;
-    float _compressDist;
+    float _compressDist;    /* Vertical compression distance. */
     double _global_ground[4];
     float _global_vel[3];
     float _casterAngle;
@@ -153,7 +179,88 @@ private:
     unsigned int _body_id;
     double _ground_rot[9];
     double _ground_trans[3];
+
+    GearVector _wheelAxle;
+    float _wheelRadius;
+    float _tyreRadius;
 };
 
+
+/* [This is internal implementation for Gear(), exposed here to allow unit
+testing.]
+
+Finds compression of gear needed to make outside of tyre rest on ground.
+
+    ground
+        Definition of ground plane; points on ground satisfy p.ground[:3] ==
+        ground[4]. Is assumed that for points below ground, p.ground[:3] is
+        positive.
+    compression
+        Movement of wheel centre at maximum compression.
+    wheel_pos
+        Centre of wheel at full extension.
+    wheel_axle
+        Direction of wheel axle; defines orientation of wheel.
+    wheel_radius
+        Distance from <wheel_pos> to centre of tubular tyre.
+    tyre_radius
+        Radius of tubular tyre.
+    bump_fn
+        Callable that returns bump value.
+    o_contact
+        Out-param for position of tyre contact after compression has been
+        applied.
+    o_compression_distance_vertical
+        Out-param vertical compression distance in metres.
+    o_compression_norm
+        Out-param compression as a fraction of maximum compression.
+
+If on ground, sets out params and returns true. Otherwise returns false.
+*/
+bool gearCompression(
+        const float (&ground)[4],
+        const GearVector& compression,
+        const float (&wheel_pos)[3],
+        const GearVector& wheel_axle,
+        float wheel_radius,
+        float tyre_radius,
+        std::function< float ()> bump_fn,
+        float (&o_contact)[3],
+        float& o_compression_distance_vertical,
+        float& o_compression_norm
+        );
+
+/* Old calculation for simple contact point.
+
+    ground
+        Definition of ground plane; points on ground satisfy p.ground[:3] ==
+        ground[4]. Is assumed that for points below ground, p.ground[:3] is
+        positive.
+    pos
+        Position of contact point.
+    compression
+        Movement of contact point at maximum compression.
+    bump_fn
+        Callable that returns bump value.
+    o_contact
+        Position of tyre contact after our compression has been applied.
+    o_compressDist
+        Compression distance in metres.
+    o_compressNorm
+        Compression as a fraction of maximum compression.
+    bump_altitude_override
+        For testing; if >= 0, we use this instead of getting a live value from
+        getBumpAltitude().
+*/
+bool gearCompressionOld(
+        const float (&ground)[4],
+        const GearVector& compression,
+        const float (&pos)[3],
+        std::function< float ()> bump_fn,
+        float (&o_contact)[3],
+        float& o_compression_distance_vertical,
+        float& o_compression_norm
+        );
+
 }; // namespace yasim
 #endif // _GEAR_HPP

src/FDM/YASim/Model.cpp

@@ -201,12 +201,9 @@ void Model::updateGround(State* s)
         Gear* g = (Gear*)_gears.get(i);
 
         // Get the point of ground contact
-        float pos[3], cmpr[3];
+        float pos[3];
-	g->getPosition(pos);
+        g->getContact(pos);
-	g->getCompression(cmpr);
         
-	Math::mul3(g->getCompressFraction(), cmpr, cmpr);
-	Math::add3(cmpr, pos, pos);
         // Transform the local coordinates of the contact point to
         // global coordinates.
         double pt[3];
@@ -442,11 +439,8 @@ void Model::newState(State* s)
         if (!g->getSubmergable())
         {
             // Get the point of ground contact
-            float pos[3], cmpr[3];
+            float pos[3];
-	    g->getPosition(pos);
+            g->getContact(pos);
-	    g->getCompression(cmpr);
-	    Math::mul3(g->getCompressFraction(), cmpr, cmpr);
-	    Math::add3(cmpr, pos, pos);
 
             // The plane transformed to local coordinates.
             double global_ground[4];

src/FDM/YASim/YASim.cxx

@@ -207,6 +207,9 @@ void YASim::init()
         node->setDoubleValue("yoffset-m", pos[1]);
         node->setDoubleValue("zoffset-m", pos[2]);
 
+        node->setDoubleValue("wheel-radius-m", g->getWheelRadius());
+        node->setDoubleValue("tyre-radius-m", g->getTyreRadius());
+
         _gearProps.push_back(GearProps(node));
     }
 
@@ -222,6 +225,7 @@ void YASim::init()
             Gear* g = airplane->getGear(i);
             float pos[3];
             g->getPosition(pos);
+            pos[2] -= (g->getWheelRadius() + g->getTyreRadius());
             if(pos[2] < minGearZ)
                 minGearZ = pos[2];
         }