X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FModel.cpp;h=80f7a8dee872f5bfa37608aa9203fce0cdcd5e19;hb=d66903e9ad63b91182ccc25d9bb82f18f8dd98b6;hp=42479f95e3104b3bfce843df94a20c944b4620c3;hpb=77ecc046762d87ca7ce7e1dd537c2ef8f2f981c0;p=flightgear.git

diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index 42479f95e..80f7a8dee 100644
--- a/src/FDM/YASim/Model.cpp
+++ b/src/FDM/YASim/Model.cpp
@@ -11,6 +11,7 @@
 #include "Surface.hpp"
 #include "Rotor.hpp"
 #include "Rotorpart.hpp"
+#include "Hitch.hpp"
 #include "Glue.hpp"
 #include "Ground.hpp"
 
@@ -75,6 +76,9 @@ Model::~Model()
     delete _ground_cb;
     delete _hook;
     delete _launchbar;
+    for(int i=0; i<_hitches.size();i++)
+        delete (Hitch*)_hitches.get(i);
+
 }
 
 void Model::getThrust(float* out)
@@ -127,6 +131,11 @@ void Model::initIteration()
         _turb->offset(toff);
     }
 
+    for(i=0; i<_hitches.size(); i++) {
+        Hitch* h = (Hitch*)_hitches.get(i);
+        h->integrate(_integrator.getInterval());
+    }
+
     
 }
 
@@ -252,6 +261,11 @@ void Model::addLaunchbar(Launchbar* launchbar)
     _launchbar = launchbar;
 }
 
+int Model::addHitch(Hitch* hitch)
+{
+    return _hitches.add(hitch);
+}
+
 void Model::setGroundCallback(Ground* ground_cb)
 {
     delete _ground_cb;
@@ -304,12 +318,33 @@ void Model::updateGround(State* s)
         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];
+        const SGMaterial* material;
+        _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material);
+        g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material);
+    }
+
+    for(i=0; i<_hitches.size(); i++) {
+        Hitch* h = (Hitch*)_hitches.get(i);
+
+        // Get the point of interest
+        float pos[3];
+        h->getPosition(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);
+        h->setGlobalGround(global_ground, global_vel);
     }
+
     for(i=0; i<_rotorgear.getRotors()->size(); i++) {
         Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
         r->findGroundEffectAltitude(_ground_cb,s);
@@ -468,8 +503,16 @@ void Model::calcForces(State* s)
         _body.addForce(contactlb, forcelb);
         _body.addForce(contacthb, forcehb);
     }
-}
 
+// The hitches
+    for(i=0; i<_hitches.size(); i++) {
+        float force[3], contact[3];
+        Hitch* h = (Hitch*)_hitches.get(i);
+        h->calcForce(_ground_cb,&_body, s);
+        h->getForce(force, contact);
+        _body.addForce(contact, force);
+    }
+}
 void Model::newState(State* s)
 {
     _s = s;
@@ -480,23 +523,26 @@ void Model::newState(State* s)
     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
-	if(dist < min)
-	    min = dist;
+        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);
+
+            // 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
+	    if(dist < min)
+	        min = dist;
+        }
     }
     _agl = min;
     if(_agl < -1) // Allow for some integration slop