Updates to the controls properties tree. This is a major update so there may be one...

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

diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index ce5ea4c5ae4ade81ac3497dc4fd5b1d0a5583d3b..8cc50c9d03aaff659d98847940bd68b2d7764694 100644 (file)
--- a/src/FDM/YASim/Model.cpp
+++ b/src/FDM/YASim/Model.cpp
@@ -25,7 +25,8 @@ void printState(State* s)
     printf("\nNEW STATE (LOCAL COORDS)\n");
     printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
     printf("o:   ");
-    for(int i=0; i<3; i++) {
+    int i;
+    for(i=0; i<3; i++) {
        if(i != 0) printf("     ");
        printf("%6.2f %6.2f %6.2f\n",
               tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
@@ -38,13 +39,17 @@ void printState(State* s)
 
 Model::Model()
 {
-    for(int i=0; i<3; i++) _wind[i] = 0;
+    int i;
+    for(i=0; i<3; i++) _wind[i] = 0;
 
     _integrator.setBody(&_body);
     _integrator.setEnvironment(this);
 
     // Default value of 30 Hz
-    _integrator.setInterval(1.0/30.0);
+    _integrator.setInterval(1.0f/30.0f);
+
+    _agl = 0;
+    _crashed = false;
 }
 
 Model::~Model()
@@ -56,7 +61,8 @@ void Model::getThrust(float* out)
 {
     float tmp[3];
     out[0] = out[1] = out[2] = 0;
-    for(int i=0; i<_thrusters.size(); i++) {
+    int i;
+    for(i=0; i<_thrusters.size(); i++) {
        Thruster* t = (Thruster*)_thrusters.get(i);
        t->getThrust(tmp);
        Math::add3(tmp, out, out);
@@ -66,9 +72,10 @@ void Model::getThrust(float* out)
 void Model::initIteration()
 {
     // Precompute torque and angular momentum for the thrusters
-    for(int i=0; i<3; i++)
+    int i;
+    for(i=0; i<3; i++)
        _gyro[i] = _torque[i] = 0;
-    for(int i=0; i<_thrusters.size(); i++) {
+    for(i=0; i<_thrusters.size(); i++) {
        Thruster* t = (Thruster*)_thrusters.get(i);
        
         // Get the wind velocity at the thruster location
@@ -77,7 +84,7 @@ void Model::initIteration()
         localWind(pos, _s, v);
 
        t->setWind(v);
-       t->setAir(_P, _T);
+       t->setAir(_pressure, _temp, _rho);
        t->integrate(_integrator.getInterval());
 
        t->getTorque(v);
@@ -95,6 +102,21 @@ void Model::iterate()
     _integrator.calcNewInterval();
 }
 
+bool Model::isCrashed()
+{
+    return _crashed;
+}
+
+void Model::setCrashed(bool crashed)
+{
+    _crashed = crashed;
+}
+
+float Model::getAGL()
+{
+    return _agl;
+}
+
 State* Model::getState()
 {
     return _s;
@@ -164,15 +186,16 @@ void Model::setGroundEffect(float* pos, float span, float mul)
 // (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];
+    int i;
+    for(i=0; i<3; i++) _ground[i] = planeNormal[i];
     _ground[3] = fromOrigin;
 }
 
-void Model::setAir(float pressure, float temp)
+void Model::setAir(float pressure, float temp, float density)
 {
-    _P = pressure;
-    _T = temp;
-    _rho = Atmosphere::calcDensity(pressure, temp);
+    _pressure = pressure;
+    _temp = temp;
+    _rho = density;
 }
 
 void Model::setWind(float* wind)
@@ -189,7 +212,8 @@ void Model::calcForces(State* s)
     // step.
     _body.setGyro(_gyro);
     _body.addTorque(_torque);
-    for(int i=0; i<_thrusters.size(); i++) {
+    int i;
+    for(i=0; i<_thrusters.size(); i++) {
        Thruster* t = (Thruster*)_thrusters.get(i);
        float thrust[3], pos[3];
        t->getThrust(thrust);
@@ -200,7 +224,7 @@ void Model::calcForces(State* s)
     // Gravity, convert to a force, then to local coordinates
     float grav[3];
     Glue::geodUp(s->pos, grav);
-    Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
+    Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
     Math::vmul33(s->orient, grav, grav);
     _body.addForce(grav);
 
@@ -208,7 +232,7 @@ void Model::calcForces(State* s)
     // point is different due to rotation.
     float faero[3];
     faero[0] = faero[1] = faero[2] = 0;
-    for(int i=0; i<_surfaces.size(); i++) {
+    for(i=0; i<_surfaces.size(); i++) {
        Surface* sf = (Surface*)_surfaces.get(i);
 
        // Vsurf = wind - velocity + (rot cross (cg - pos))
@@ -233,11 +257,12 @@ void Model::calcForces(State* s)
     // Account for ground effect by multiplying the vertical force
     // component by an amount linear with the fraction of the wingspan
     // above the ground.
-    float dist = ground[4] - Math::dot3(ground, _wingCenter);
+    float dist = ground[3] - Math::dot3(ground, _wingCenter);
     if(dist > 0 && dist < _groundEffectSpan) {
        float fz = Math::dot3(faero, ground);
-       Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
-                  ground, faero);
+        fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
+        fz *= _groundEffect;
+       Math::mul3(fz, ground, faero);
        _body.addForce(faero);
     }
     
@@ -247,7 +272,7 @@ void Model::calcForces(State* s)
     Math::vmul33(s->orient, s->v, lv);
 
     // The landing gear
-    for(int i=0; i<_gears.size(); i++) {
+    for(i=0; i<_gears.size(); i++) {
        float force[3], contact[3];
        Gear* g = (Gear*)_gears.get(i);
        g->calcForce(&_body, lv, lrot, ground);
@@ -263,19 +288,27 @@ void Model::newState(State* s)
     //printState(s);
 
     // Some simple collision detection
+    float min = 1e8;
     float ground[4], pos[3], cmpr[3];
     ground[3] = localGround(s, ground);
-    for(int i=0; i<_gears.size(); i++) {
+    int i;
+    for(i=0; i<_gears.size(); i++) {
        Gear* g = (Gear*)_gears.get(i);
+
+       // Get the point of ground contact
        g->getPosition(pos);
        g->getCompression(cmpr);
+       Math::mul3(g->getCompressFraction(), cmpr, cmpr);
        Math::add3(cmpr, pos, pos);
        float dist = ground[3] - Math::dot3(pos, ground);
-       if(dist < 0) {
-           printf("CRASH: gear %d\n", i);
-           *(int*)0=0;
-       }
+
+       // Find the lowest one
+       if(dist < min)
+           min = dist;
     }
+    _agl = min;
+    if(_agl < -1) // Allow for some integration slop
+       _crashed = true;
 }
 
 // Returns a unit "down" vector for the ground in out, and the