diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 6eec6fe8a..f53c02e7d 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/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,7 +653,11 @@ void Airplane::solveGear() GearRec* gr = (GearRec*)_gears.get(i); Gear* g = gr->gear; g->getPosition(pos); - Math::sub3(cg, pos, 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()) total += gr->wgt; diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 1845399ee..97b94101e 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -6,6 +6,8 @@ #include #include +#include + #include
#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)); diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp index 20c9409ca..b198bd3a0 100644 --- a/src/FDM/YASim/Gear.cpp +++ b/src/FDM/YASim/Gear.cpp @@ -3,6 +3,8 @@ # include "config.h" #endif +#include + #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 to unit vector along , or all zeros if is zero +length. Returns length of . */ +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. + + 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 (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 , 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 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 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 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 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. - float glvel[3]; - s->globalToLocal(_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); - float BumpAltitude=0; - if (a 0) { + bool on_ground = gearCompression( + ground, + _cmpr, + _pos, + _wheelAxle, + _wheelRadius, + _tyreRadius, + [this] { return this->getBumpAltitude(); }, + _contact, + _compressDist, + _frac + ); + if (!on_ground) { _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 - // 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)+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]; - + // The velocity of the contact patch transformed to local coordinates. + float glvel[3]; + s->globalToLocal(_global_vel, glvel); + // Turn _cmpr into a unit vector and a magnitude - float cmpr[3]; - float clen = Math::mag3(_cmpr); - Math::mul3(1/clen, _cmpr, cmpr); + const float (&cmpr)[3] = _cmpr.unit; + const float &clen = _cmpr.magnitude; // 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; diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp index f8ace92d9..59540365e 100644 --- a/src/FDM/YASim/Gear.hpp +++ b/src/FDM/YASim/Gear.hpp @@ -2,6 +2,9 @@ #define _GEAR_HPP #include "Math.hpp" +#include + + 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 getCompression(float* out) { Math::set3(_cmpr, out); } + void setCompression(float* compression); + 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 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 diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 102be0877..3dd197003 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -198,15 +198,12 @@ void Model::updateGround(State* s) int i; // The landing gear for(i=0; i<_gears.size(); i++) { - Gear* g = (Gear*)_gears.get(i); + Gear* g = (Gear*)_gears.get(i); - // Get the point of ground contact - float pos[3], cmpr[3]; - g->getPosition(pos); - g->getCompression(cmpr); - - Math::mul3(g->getCompressFraction(), cmpr, cmpr); - Math::add3(cmpr, pos, pos); + // Get the point of ground contact + float pos[3]; + g->getContact(pos); + // Transform the local coordinates of the contact point to // global coordinates. double pt[3]; @@ -437,33 +434,30 @@ void Model::newState(State* s) float min = 1e8; int i; for(i=0; i<_gears.size(); i++) { - Gear* g = (Gear*)_gears.get(i); + Gear* g = (Gear*)_gears.get(i); if (!g->getSubmergable()) { - // Get the point of ground contact - float pos[3], cmpr[3]; - g->getPosition(pos); - g->getCompression(cmpr); - Math::mul3(g->getCompressFraction(), cmpr, cmpr); - Math::add3(cmpr, pos, pos); + // Get the point of ground contact + float pos[3]; + g->getContact(pos); // The plane transformed to local coordinates. double global_ground[4]; g->getGlobalGround(global_ground); float ground[4]; s->planeGlobalToLocal(global_ground, ground); - float dist = ground[3] - Math::dot3(pos, ground); + float dist = ground[3] - Math::dot3(pos, ground); - // Find the lowest one - if(dist < min) - min = dist; + // Find the lowest one + if(dist < min) + min = dist; } g->updateStuckPoint(s); } _agl = min; if(_agl < -1) // Allow for some integration slop - _crashed = true; + _crashed = true; } // Calculates the airflow direction at the given point and for the diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index e1c523ef6..25dc7e626 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/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]; }