Mathias Frohlich: Add carrier capabilities for YASim aircraft.

src/FDM/YASim/Airplane.cpp

@@ -137,6 +137,16 @@ Gear* Airplane::getGear(int g)
     return ((GearRec*)_gears.get(g))->gear;
 }
 
+Hook* Airplane::getHook()
+{
+    return _model.getHook();
+}
+
+Launchbar* Airplane::getLaunchbar()
+{
+    return _model.getLaunchbar();
+}
+
 void Airplane::updateGearState()
 {
     for(int i=0; i<_gears.size(); i++) {
@@ -285,6 +295,16 @@ void Airplane::addGear(Gear* gear)
     _gears.add(g);
 }
 
+void Airplane::addHook(Hook* hook)
+{
+    _model.addHook(hook);
+}
+
+void Airplane::addLaunchbar(Launchbar* launchbar)
+{
+    _model.addLaunchbar(launchbar);
+}
+
 void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
 {
     ThrustRec* t = new ThrustRec();
@@ -595,10 +615,6 @@ void Airplane::compileContactPoints()
 
 void Airplane::compile()
 {
-    double ground[3];
-    ground[0] = 0; ground[1] = 0; ground[2] = 1;
-    _model.setGroundPlane(ground, -100000);
-
     RigidBody* body = _model.getBody();
     int firstMass = body->numMasses();
 

src/FDM/YASim/Airplane.hpp

@@ -10,6 +10,8 @@
 namespace yasim {
 
 class Gear;
+class Hook;
+class Launchbar;
 class Thruster;
 
 class Airplane {
@@ -42,6 +44,8 @@ public:
                      float taper=1, float mid=0.5);
     int addTank(float* pos, float cap, float fuelDensity);
     void addGear(Gear* g);
+    void addHook(Hook* h);
+    void addLaunchbar(Launchbar* l);
     void addThruster(Thruster* t, float mass, float* cg);
     void addBallast(float* pos, float mass);
 
@@ -59,6 +63,8 @@ public:
 
     int numGear();
     Gear* getGear(int g);
+    Hook* getHook();
+    Launchbar* getLaunchbar();
 
     int numThrusters() { return _thrusters.size(); }
     Thruster* getThruster(int n) {

src/FDM/YASim/BodyEnvironment.hpp

@@ -2,6 +2,7 @@
 #define _BODYENVIRONMENT_HPP
 
 #include "RigidBody.hpp"
+#include "Math.hpp"
 
 namespace yasim {
 
@@ -29,6 +30,39 @@ struct State {
                 orient[3*i+j] = i==j ? 1.0f : 0.0f;
         }
     }
+
+    void posLocalToGlobal(float* lpos, double *gpos) {
+        float tmp[3];
+        Math::tmul33(orient, lpos, tmp);
+        gpos[0] = tmp[0] + pos[0];
+        gpos[1] = tmp[1] + pos[1];
+        gpos[2] = tmp[2] + pos[2];
+    }
+    void posGlobalToLocal(double* gpos, float *lpos) {
+        lpos[0] = gpos[0] - pos[0];
+        lpos[1] = gpos[1] - pos[1];
+        lpos[2] = gpos[2] - pos[2];
+        Math::vmul33(orient, lpos, lpos);
+    }
+    void velLocalToGlobal(float* lvel, float *gvel) {
+        Math::tmul33(orient, lvel, gvel);
+    }
+    void velGlobalToLocal(float* gvel, float *lvel) {
+        Math::vmul33(orient, gvel, lvel);
+    }
+
+    void planeGlobalToLocal(double* gplane, float *lplane) {
+      // First the normal vector transformed to local coordinates.
+      lplane[0] = -gplane[0];
+      lplane[1] = -gplane[1];
+      lplane[2] = -gplane[2];
+      Math::vmul33(orient, lplane, lplane);
+
+      // Then the distance from the plane to the Aircraft's origin.
+      lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1]
+                          + pos[2]*gplane[2] - gplane[3]);
+    }
+
 };
 
 //

src/FDM/YASim/ControlMap.cpp

@@ -4,6 +4,8 @@
 #include "PistonEngine.hpp"
 #include "TurbineEngine.hpp"
 #include "Gear.hpp"
+#include "Hook.hpp"
+#include "Launchbar.hpp"
 #include "Wing.hpp"
 #include "Rotor.hpp"
 #include "Math.hpp"
@@ -198,6 +200,8 @@ void ControlMap::applyControls(float dt)
 	case BRAKE:    ((Gear*)obj)->setBrake(lval);               break;
 	case STEER:    ((Gear*)obj)->setRotation(lval);            break;
 	case EXTEND:   ((Gear*)obj)->setExtension(lval);           break;
+	case HEXTEND:  ((Hook*)obj)->setExtension(lval);           break;
+	case LEXTEND:  ((Launchbar*)obj)->setExtension(lval);      break;
 	case CASTERING:((Gear*)obj)->setCastering(lval != 0);      break;
 	case SLAT:     ((Wing*)obj)->setSlat(lval);                break;
 	case FLAP0:    ((Wing*)obj)->setFlap0(lval, rval);         break;

src/FDM/YASim/FGFDM.cpp

@@ -7,6 +7,8 @@
 #include "Jet.hpp"
 #include "SimpleJet.hpp"
 #include "Gear.hpp"
+#include "Hook.hpp"
+#include "Launchbar.hpp"
 #include "Atmosphere.hpp"
 #include "PropEngine.hpp"
 #include "Propeller.hpp"
@@ -46,6 +48,8 @@ static const float NM2FTLB = (1/(LBS2N*FT2M));
 
 FGFDM::FGFDM()
 {
+    _vehicle_radius = 0.0f;
+
     _nextEngine = 0;
 
     // Map /controls/flight/elevator to the approach elevator control.  This
@@ -231,6 +235,9 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[1] = attrf(a, "y");
 	v[2] = attrf(a, "z");
 	g->setPosition(v);
+        float nrm = Math::mag3(v);
+        if (_vehicle_radius < nrm)
+            _vehicle_radius = nrm;
 	v[0] = 0;
 	v[1] = 0;
 	v[2] = attrf(a, "compression", 1);
@@ -242,7 +249,36 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
         g->setDamping(attrf(a, "damp", 1));
 	_airplane.addGear(g);
     } else if(eq(name, "hook")) {
+	Hook* h = new Hook();
+	_currObj = h;
+	v[0] = attrf(a, "x");
+	v[1] = attrf(a, "y");
+	v[2] = attrf(a, "z");
+	h->setPosition(v);
+        float length = attrf(a, "length", 1.0);
+        h->setLength(length);
+        float nrm = length+Math::mag3(v);
+        if (_vehicle_radius < nrm)
+            _vehicle_radius = nrm;
+        h->setDownAngle(attrf(a, "down-angle", 70) * DEG2RAD);
+        h->setUpAngle(attrf(a, "up-angle", 0) * DEG2RAD);
+ 	_airplane.addHook(h);
     } else if(eq(name, "launchbar")) {
+	Launchbar* l = new Launchbar();
+	_currObj = l;
+	v[0] = attrf(a, "x");
+	v[1] = attrf(a, "y");
+	v[2] = attrf(a, "z");
+	l->setLaunchbarMount(v);
+	v[0] = attrf(a, "holdback-x", v[0]);
+	v[1] = attrf(a, "holdback-y", v[1]);
+	v[2] = attrf(a, "holdback-z", v[2]);
+	l->setHoldbackMount(v);
+        float length = attrf(a, "length", 1.0);
+        l->setLength(length);
+        l->setDownAngle(attrf(a, "down-angle", 30) * DEG2RAD);
+        l->setUpAngle(attrf(a, "up-angle", -30) * DEG2RAD);
+ 	_airplane.addLaunchbar(l);
     } else if(eq(name, "fuselage")) {
 	float b[3];
 	v[0] = attrf(a, "ax");

src/FDM/YASim/FGFDM.hpp

@@ -27,6 +27,8 @@ public:
     // XML parsing callback from XMLVisitor
     virtual void startElement(const char* name, const XMLAttributes &atts);
 
+    float getVehicleRadius(void) const { return _vehicle_radius; }
+
 private:
     struct AxisRec { char* name; int handle; };
     struct EngRec { char* prefix; Thruster* eng; };
@@ -72,6 +74,9 @@ private:
     // Output properties for the ControlMap
     Vector _controlProps;
 
+    // Radius of the vehicle, for intersection testing.
+    float _vehicle_radius;
+
     // Parsing temporaries
     void* _currObj;
     bool _cruiseCurr;

src/FDM/YASim/Gear.cpp

@@ -1,4 +1,5 @@
 #include "Math.hpp"
+#include "BodyEnvironment.hpp"
 #include "RigidBody.hpp"
 
 #include "Gear.hpp"
@@ -17,6 +18,12 @@ Gear::Gear()
     _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,7 +169,13 @@ 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.
@@ -164,6 +190,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 +209,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):
@@ -226,11 +254,18 @@ 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
+	// Correct for a rotation
-	Math::mul3(_rot, steer, tmp);
+        float srot = Math::sin(_rot);
-	Math::add3(tmp, skid, skid);
+        float crot = Math::cos(_rot);
-	Math::unit3(skid, skid);
+        float tx = steer[0];
-	Math::cross3(skid, gup, steer);
+        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);

src/FDM/YASim/Gear.hpp

@@ -4,6 +4,7 @@
 namespace yasim {
 
 class RigidBody;
+class State;
 
 // A landing gear has the following parameters:
 //
@@ -38,9 +39,11 @@ public:
     void setRotation(float rotation);
     void setExtension(float extension);
     void setCastering(bool castering);
+    void setGlobalGround(double* global_ground, float* global_vel);
 
     void getPosition(float* out);
     void getCompression(float* out);
+    void getGlobalGround(double* global_ground);
     float getSpring();
     float getDamping();
     float getStaticFriction();
@@ -54,7 +57,7 @@ public:
     // vector, and a ground plane (all specified in local coordinates)
     // and make a force and point of application (i.e. ground contact)
     // available via getForce().
-    void calcForce(RigidBody* body, float* v, float* rot, float* ground);
+    void calcForce(RigidBody* body, State* s, float* v, float* rot);
 
     // Computed values: total force, weight-on-wheels (force normal to
     // ground) and compression fraction.
@@ -79,6 +82,8 @@ private:
     float _contact[3];
     float _wow;
     float _frac;
+    double _global_ground[4];
+    float _global_vel[3];
 };
 
 }; // namespace yasim

src/FDM/YASim/Makefile.am

@@ -14,6 +14,9 @@ SHARED_SOURCE_FILES = \
         FGFDM.cpp FGFDM.hpp \
         Gear.cpp Gear.hpp \
         Glue.cpp Glue.hpp \
+        Ground.cpp Ground.hpp \
+        Hook.cpp Hook.hpp \
+        Launchbar.cpp Launchbar.hpp \
         Integrator.cpp Integrator.hpp \
         Jet.cpp Jet.hpp \
         Math.cpp Math.hpp \
@@ -36,7 +39,7 @@ SHARED_SOURCE_FILES = \
 
 noinst_LIBRARIES = libYASim.a
 
-libYASim_a_SOURCES = YASim.cxx YASim.hxx $(SHARED_SOURCE_FILES)
+libYASim_a_SOURCES = YASim.cxx YASim.hxx FGGround.cpp FGGround.hpp $(SHARED_SOURCE_FILES)
 
 bin_PROGRAMS = yasim
 noinst_PROGRAMS = proptest

src/FDM/YASim/Model.cpp

@@ -6,11 +6,14 @@
 #include "Propeller.hpp"
 #include "PistonEngine.hpp"
 #include "Gear.hpp"
+#include "Hook.hpp"
+#include "Launchbar.hpp"
 #include "Surface.hpp"
 #include "Rotor.hpp"
 #include "Rotorpart.hpp"
 #include "Rotorblade.hpp"
 #include "Glue.hpp"
+#include "Ground.hpp"
 
 #include "Model.hpp"
 namespace yasim {
@@ -54,11 +57,17 @@ Model::Model()
     _agl = 0;
     _crashed = false;
     _turb = 0;
+    _ground_cb = new Ground();
+    _hook = 0;
+    _launchbar = 0;
 }
 
 Model::~Model()
 {
     // FIXME: who owns these things?  Need a policy
+    delete _ground_cb;
+    delete _hook;
+    delete _launchbar;
 }
 
 void Model::getThrust(float* out)
@@ -81,8 +90,9 @@ void Model::initIteration()
 	_gyro[i] = _torque[i] = 0;
 
     // Need a local altitude for the wind calculation
-    float dummy[3];
+    float lground[4];
-    float alt = Math::abs(localGround(_s, dummy));
+    _s->planeGlobalToLocal(_global_ground, lground);
+    float alt = Math::abs(lground[3]);
 
     for(i=0; i<_thrusters.size(); i++) {
 	Thruster* t = (Thruster*)_thrusters.get(i);
@@ -109,6 +119,8 @@ void Model::initIteration()
         Math::mul3(_integrator.getInterval(), _wind, toff);
         _turb->offset(toff);
     }
+
+    
 }
 
 // FIXME: This method looks to me like it's doing *integration*, not
@@ -205,6 +217,16 @@ int Model::addThruster(Thruster* t)
     return _thrusters.add(t);
 }
 
+Hook* Model::getHook(void)
+{
+    return _hook;
+}
+
+Launchbar* Model::getLaunchbar(void)
+{
+    return _launchbar;
+}
+
 int Model::numThrusters()
 {
     return _thrusters.size();
@@ -243,6 +265,27 @@ int Model::addGear(Gear* gear)
     return _gears.add(gear);
 }
 
+void Model::addHook(Hook* hook)
+{
+    _hook = hook;
+}
+
+void Model::addLaunchbar(Launchbar* launchbar)
+{
+    _launchbar = launchbar;
+}
+
+void Model::setGroundCallback(Ground* ground_cb)
+{
+    delete _ground_cb;
+    _ground_cb = ground_cb;
+}
+
+Ground* Model::getGroundCallback(void)
+{
+    return _ground_cb;
+}
+
 void Model::setGroundEffect(float* pos, float span, float mul)
 {
     Math::set3(pos, _wingCenter);
@@ -250,16 +293,6 @@ void Model::setGroundEffect(float* pos, float span, float mul)
     _groundEffect = mul;
 }
 
-// The first three elements are a unit vector pointing from the global
-// origin to the plane, the final element is the distance from the
-// origin (the radius of the earth, in most implementations).  So
-// (v dot _ground)-_ground[3] gives the distance AGL.
-void Model::setGroundPlane(double* planeNormal, double fromOrigin)
-{
-    for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
-    _ground[3] = fromOrigin;
-}
-
 void Model::setAir(float pressure, float temp, float density)
 {
     _pressure = pressure;
@@ -272,6 +305,54 @@ void Model::setWind(float* wind)
     Math::set3(wind, _wind);
 }
 
+void Model::updateGround(State* s)
+{
+    float dummy[3];
+    _ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
+
+    int i;
+    // The landing gear
+    for(i=0; i<_gears.size(); 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);
+        // Transform the local coordinates of the contact point to
+        // global coordinates.
+        double pt[3];
+        s->posLocalToGlobal(pos, pt);
+
+        // Ask for the ground plane in the global coordinate system
+        double global_ground[4];
+        float global_vel[3];
+        _ground_cb->getGroundPlane(pt, global_ground, global_vel);
+        g->setGlobalGround(global_ground, global_vel);
+    }
+
+    // The arrester hook
+    if(_hook) {
+        double pt[3];
+        _hook->getTipGlobalPosition(s, pt);
+        double global_ground[4];
+        _ground_cb->getGroundPlane(pt, global_ground, dummy);
+        _hook->setGlobalGround(global_ground);
+    }
+
+    // The launchbar/holdback
+    if(_launchbar) {
+        double pt[3];
+        _launchbar->getTipGlobalPosition(s, pt);
+        double global_ground[4];
+        _ground_cb->getGroundPlane(pt, global_ground, dummy);
+        _launchbar->setGlobalGround(global_ground);
+    }
+}
+
 void Model::calcForces(State* s)
 {
     // Add in the pre-computed stuff.  These values aren't part of the
@@ -295,7 +376,7 @@ void Model::calcForces(State* s)
     // from the local origin along that vector to the ground plane
     // (negative for objects "above" the ground)
     float ground[4];
-    ground[3] = localGround(s, ground);
+    s->planeGlobalToLocal(_global_ground, ground);
     float alt = Math::abs(ground[3]);
 
     // Gravity, convert to a force, then to local coordinates
@@ -380,10 +461,29 @@ void Model::calcForces(State* s)
     for(i=0; i<_gears.size(); i++) {
 	float force[3], contact[3];
 	Gear* g = (Gear*)_gears.get(i);
-	g->calcForce(&_body, lv, lrot, ground);
+
+	g->calcForce(&_body, s, lv, lrot);
 	g->getForce(force, contact);
 	_body.addForce(contact, force);
     }
+
+    // The arrester hook
+    if(_hook) {
+	float v[3], rot[3], glvel[3], ground[3];
+        _hook->calcForce(_ground_cb, &_body, s, lv, lrot);
+	float force[3], contact[3];
+        _hook->getForce(force, contact);
+        _body.addForce(contact, force);
+    }
+
+    // The launchbar/holdback
+    if(_launchbar) {
+	float v[3], rot[3], glvel[3], ground[3];
+        _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
+	float force[3], contact[3];
+        _launchbar->getForce(force, contact);
+        _body.addForce(contact, force);
+    }
 }
 
 void Model::newState(State* s)
@@ -392,17 +492,22 @@ void Model::newState(State* s)
 
     // Some simple collision detection
     float min = 1e8;
-    float ground[4], pos[3], cmpr[3];
-    ground[3] = localGround(s, ground);
     int i;
     for(i=0; i<_gears.size(); 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);
+
+        // 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);
 
 	// Find the lowest one
@@ -414,28 +519,6 @@ void Model::newState(State* s)
 	_crashed = true;
 }
 
-// Returns a unit "down" vector for the ground in out, and the
-// distance from the local origin to the ground as the return value.
-// So for a given position V, "dist - (V dot out)" will be the height
-// AGL.
-float Model::localGround(State* s, float* out)
-{
-    // Get the ground's "down" vector, this can be in floats, because
-    // we don't need positioning accuracy.  The direction has plenty
-    // of accuracy after truncation.
-    out[0] = -(float)_ground[0];
-    out[1] = -(float)_ground[1];
-    out[2] = -(float)_ground[2];
-    Math::vmul33(s->orient, out, out);
-
-    // The distance from the ground to the Aircraft's origin:
-    double dist = (s->pos[0]*_ground[0]
-                   + s->pos[1]*_ground[1]
-                   + s->pos[2]*_ground[2] - _ground[3]);
-
-    return (float)dist;
-}
-
 // Calculates the airflow direction at the given point and for the
 // specified aircraft velocity.
 void Model::localWind(float* pos, State* s, float* out, float alt)
@@ -449,8 +532,8 @@ void Model::localWind(float* pos, State* s, float* out, float alt)
         Math::tmul33(s->orient, pos, tmp);
         for(int i=0; i<3; i++) {
             gpos[i] = s->pos[i] + tmp[i];
-            up[i] = _ground[i];
         }
+        Glue::geodUp(gpos, up);
         _turb->getTurbulence(gpos, alt, up, lwind);
         Math::add3(_wind, lwind, lwind);
     } else {

src/FDM/YASim/Model.hpp

@@ -17,6 +17,9 @@ class Rotorpart;
 class Rotorblade;
 class Rotor;
 class Gear;
+class Ground;
+class Hook;
+class Launchbar;
 
 class Model : public BodyEnvironment {
 public:
@@ -44,11 +47,15 @@ public:
     int addRotorblade(Rotorblade* rblade);
     int addRotor(Rotor* rotor);
     int addGear(Gear* gear);
+    void addHook(Hook* hook);
+    void addLaunchbar(Launchbar* launchbar);
     Surface* getSurface(int handle);
     Rotorpart* getRotorpart(int handle);
     Rotorblade* getRotorblade(int handle);
     Rotor* getRotor(int handle);
     Gear* getGear(int handle);
+    Hook* getHook(void);
+    Launchbar* getLaunchbar(void);
 
     // Semi-private methods for use by the Airplane solver.
     int numThrusters();
@@ -57,14 +64,18 @@ public:
     void initIteration();
     void getThrust(float* out);
 
+    void setGroundCallback(Ground* ground_cb);
+    Ground* getGroundCallback(void);
+
     //
     // Per-iteration settables
     //
-    void setGroundPlane(double* planeNormal, double fromOrigin);
     void setGroundEffect(float* pos, float span, float mul);
     void setWind(float* wind);
     void setAir(float pressure, float temp, float density);
 
+    void updateGround(State* s);
+
     // BodyEnvironment callbacks
     virtual void calcForces(State* s);
     virtual void newState(State* s);
@@ -73,7 +84,6 @@ private:
     void initRotorIteration();
     void calcGearForce(Gear* g, float* v, float* rot, float* ground);
     float gearFriction(float wgt, float v, Gear* g);
-    float localGround(State* s, float* out);
     void localWind(float* pos, State* s, float* out, float alt);
 
     Integrator _integrator;
@@ -87,12 +97,15 @@ private:
     Vector _rotorblades;
     Vector _rotors;
     Vector _gears;
+    Hook* _hook;
+    Launchbar* _launchbar;
 
     float _groundEffectSpan;
     float _groundEffect;
     float _wingCenter[3];
 
-    double _ground[4];
+    Ground* _ground_cb;
+    double _global_ground[4];
     float _pressure;
     float _temp;
     float _rho;

src/FDM/YASim/YASim.cxx

@@ -16,6 +16,9 @@
 #include "Integrator.hpp"
 #include "Glue.hpp"
 #include "Gear.hpp"
+#include "Hook.hpp"
+#include "Launchbar.hpp"
+#include "FGGround.hpp"
 #include "PropEngine.hpp"
 #include "PistonEngine.hpp"
 
@@ -43,6 +46,7 @@ YASim::YASim(double dt)
 
     _dt = dt;
 
+    _fdm->getAirplane()->getModel()->setGroundCallback( new FGGround(this) );
     _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
 }
 
@@ -185,12 +189,32 @@ void YASim::update(double dt)
         return;
     }
 
+    // ground.  Calculate a cartesian coordinate for the ground under
+    // us, find the (geodetic) up vector normal to the ground, then
+    // use that to find the final (radius) term of the plane equation.
+    float v[3] = { get_uBody()*FT2M, get_vBody()*FT2M, get_wBody()*FT2M };
+    float lat = get_Latitude(); float lon = get_Longitude();
+    double xyz[3];
+    sgGeodToCart(lat, lon, 0.0, xyz);
+    // build the environment cache.
+    float vr = _fdm->getVehicleRadius();
+    vr += 2.0*dt*Math::mag3(v);
+    prepare_ground_cache_m( 0.0, xyz, vr );
+
+    // Track time increments.
+    FGGround* gr
+      = (FGGround*)_fdm->getAirplane()->getModel()->getGroundCallback();
+
     int i;
     for(i=0; i<iterations; i++) {
+        gr->setTimeOffset(iterations*_dt);
         copyToYASim(false);
         _fdm->iterate(_dt);
         copyFromYASim();
     }
+
+    // Reset the time increment.
+    gr->setTimeOffset(0.0);
 }
 
 void YASim::copyToYASim(bool copyState)
@@ -209,10 +233,6 @@ void YASim::copyToYASim(bool copyState)
     wind[1] = get_V_east_airmass() * FT2M * -1.0;
     wind[2] = get_V_down_airmass() * FT2M * -1.0;
 
-    // Get ground elevation
-    double ground = fgGetDouble("/position/ground-elev-m");
-    // cout << "YASIM: ground = " << ground << endl;
-
     float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA;
     float temp = fgGetFloat("/environment/temperature-degc") + 273.15;
     float dens = fgGetFloat("/environment/density-slugft3") 
@@ -279,19 +299,16 @@ void YASim::copyToYASim(bool copyState)
     Math::tmul33(xyz2ned, wind, wind);
     model->setWind(wind);
 
-    // ground.  Calculate a cartesian coordinate for the ground under
-    // us, find the (geodetic) up vector normal to the ground, then
-    // use that to find the final (radius) term of the plane equation.
-    double xyz[3], gplane[3]; float up[3];
-    sgGeodToCart(lat, lon, ground, xyz);
-    Glue::geodUp(lat, lon, up); // FIXME, needless reverse computation...
-    int i;
-    for(i=0; i<3; i++) gplane[i] = up[i];
-    double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
-    model->setGroundPlane(gplane, rad);
-
     // air
     model->setAir(pressure, temp, dens);
+
+    // Query a ground plane for each gear/hook/launchbar and
+    // write that value into the corresponding class.
+    _fdm->getAirplane()->getModel()->updateGround(&s);
+
+    Launchbar* l = model->getLaunchbar();
+    if (l)
+        l->setLaunchCmd(0.0<fgGetFloat("/controls/gear/catapult-launch-cmd"));
 }
 
 // All the settables:
@@ -443,4 +460,16 @@ void YASim::copyFromYASim()
 	node->setBoolValue("wow", g->getCompressFraction() != 0);
 	node->setFloatValue("compression-norm", g->getCompressFraction());
     }
+
+    Hook* h = airplane->getHook();
+    if(h) {
+	SGPropertyNode * node = fgGetNode("gear/tailhook", 0, true);
+	node->setFloatValue("position-norm", h->getCompressFraction());
+    }
+
+    Launchbar* l = airplane->getLaunchbar();
+    if(l) {
+	SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true);
+	node->setFloatValue("position-norm", l->getCompressFraction());
+    }
 }

src/FDM/groundcache.cxx

@@ -143,14 +143,16 @@ FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia,
 {
   // Extract data from the leaf which is just copied.
   ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices();
+  ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals();
   // Create a new leaf.
-  ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 0, 0, ia );
+  ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 0, 0, ia );
   // Clones data ...
   vtxa->removeUnusedVertices();
   // Apply transform. We won't store transforms in our cache.
   vtxa->transform( xform );
   // Check for magic texture names object names and such ...
   vtxa->setUserData( extractGroundProperty( l ) );
+  vtxa->setCullFace( l->getCullFace() );
   // Finally append to cache.
   cache_root.addKid((ssgEntity*)vtxa);
 }
@@ -528,10 +530,18 @@ FGGroundCache::get_agl(double t, const double dpt[3],
       sgVec3 isecpoint;
       if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
            sgPointInTriangle3( isecpoint, tri ) ) {
+        // Only accept surfaces with the normal pointing upwards.
+        // For double sided surfaces flip the normal in this case.
+        float dirDot = sgScalarProductVec3(plane, dir);
+        if ( dirDot >= 0 && va->getCullFace() == 1 ) {
+          sgScaleVec4( plane, -1 );
+          dirDot = -dirDot;
+        }
+
         // Check for the closest intersection point.
         // FIXME: is this the right one?
         double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
-        if ( newSqdist < sqdist ) {
+        if ( newSqdist < sqdist && dirDot < 0 ) {
           sqdist = newSqdist;
           ret = true;
           // Save the new potential intersection point.