X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FModel.cpp;h=8cc50c9d03aaff659d98847940bd68b2d7764694;hb=c3cefaf883ddadb467c0e5da7f966558cbc2355b;hp=ce5ea4c5ae4ade81ac3497dc4fd5b1d0a5583d3b;hpb=4c422bbe6d3160d4a46058da87942c6317ce3dca;p=flightgear.git

diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index ce5ea4c5a..8cc50c9d0 100644
--- 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