Logo Search packages:      
Sourcecode: acm version File versions  Download package

gear.c

/*
 *  acm : an aerial combat simulator for X
 *  Copyright (C) 1991-1997  Riley Rainey
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 dated June, 1991.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program;  if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave., Cambridge, MA 02139, USA.
 */
#include <stdio.h>
#include <math.h>
#include "pm.h"

extern void euler(craft *);

/*
 *  Determine the time, relative to the beginning of this time interval,
 *  that the landing gear first make contact with the ground.  Return -1
 *  if they don't during this interval.
 */

#ifdef FLIGHTDEBUG
#define DEBUG1
extern int debug;

#endif

double
groundContactTime(craft * c, VPoint * contactSg)
{
      register craftType *p = c->cinfo;
      register double tm, tn;
      VPoint    v, s0, s0n, Sg, gear;
      double    local_z = 0.0;

/*
 *  In this function, our computation of wheel position differs slightly
 *  with the one used in groundDynamics().  This can introduce a problem where
 *  this procedure thinks we are not in contact with the ground while
 *  groundDynamics() does.  We'll define an error margin to prevent that
 *  condition.
 */
#define EPSILON   0.01
#define geardown (M_PI / 2.0)

#ifdef DEBUG1
      double    cm, cn, cosTheta, sinTheta;

#endif

      tm = tn = 2.0;

#ifdef FLAT_WORLD
      Sg = c->prevSg;
      v.x = c->Sg.x - c->prevSg.x;
      v.y = c->Sg.y - c->prevSg.y;
      v.z = c->Sg.z - c->prevSg.z;
#else
      Sg.x = Sg.y = 0.0;
      local_z = localAltitude(&c->prevSg, &c->prevw);
      Sg.z = -METERStoFEET(c->w.z - local_z);
      c->localTerrain_feet = METERStoFEET( local_z );
      v.x = c->Cg.x;
      v.y = c->Cg.y;
      v.z = -METERStoFEET(c->w.z - c->prevw.z);
/*      printf ("Sg.z = %g, v.z = %g\n", Sg.z, v.z); */
#endif

/*
 *  If we're not on the ground now, and we're not descending, we won't touch
 *  down during this time interval.
 */

      if (v.z <= 0.0) {
            return -1.0;
      }

/*
 *  Determine the point in world axes relative to the CG that represents the
 *  "bottom" of the fully extended main gear with a fully extended oleo.
 *  The determine if (and when) it makes contact with the ground.
 *
 *  Remember, the main gear are considered one unit for purposes of ACM.
 */

      gear = p->rm;
      gear.z += p->Gm + p->cmMax;
      VTransform(&gear, &(c->trihedral), &s0);
#ifdef FLAT_WORLD
      s0.x += c->prevSg.x;
      s0.y += c->prevSg.y;
      s0.z += c->prevSg.z;
#else
      s0.z += -METERStoFEET(c->prevw.z - local_z);
#endif
      if (s0.z > (0.0 + EPSILON)) {
            *contactSg = c->prevSg;
            c->flags |= FL_GND_CONTACT;
#ifdef DEBUG1
            printf("mains already in contact\n");
            cosTheta = cos(-c->curPitch);
            sinTheta = sin(-c->curPitch);
            cm = -(contactSg->z + p->rm.x * sinTheta) / cosTheta -
                  p->Gm - p->rm.z;
            cn = -(contactSg->z + p->rn.x * sinTheta) / cosTheta -
                  p->Gn - p->rn.z;
            printf("cm/cn cmMax/cnMax %.4f %.4f %.4f %.4f\n\n",
                     cm, cn, p->cmMax, p->cnMax);
#endif
            return 0.0;
      }

      if ((s0.z + v.z) > 0.0) {
            tm = -s0.z / v.z;
            if (c->curGear[1] != geardown || c->curGear[2] != geardown) {
                  killPlayerEx(c,
                                 "Your main landing gear wasn't down and locked\n",
                                     "Either you forgot to extend the gear, or it was damaged in combat.");
                  return -1;
            }
      }

/*
 *  Now do the same thing for the nose gear.
 */

      gear = p->rn;
      gear.z += p->Gn + p->cnMax;
      VTransform(&gear, &(c->trihedral), &s0n);
#ifdef FLAT_WORLD
      s0n.x += c->prevSg.x;
      s0n.y += c->prevSg.y;
      s0n.z += c->prevSg.z;
#else
      s0n.z -= METERStoFEET(c->prevw.z - local_z);
#endif

      if (s0n.z > (0.0 + EPSILON)) {
            *contactSg = c->prevSg;
            c->flags |= FL_GND_CONTACT;
#ifdef DEBUG1
            printf("nose already in contact\n");
#endif
            return 0.0;
      }

      if ((s0n.z + v.z) > 0.0) {
            tn = -s0n.z / v.z;
            if (c->curGear[0] != geardown) {
                  killPlayerEx(c,
                                     "Your nose gear wasn't down and locked",
                                     "Either you forgot to extend the gear, or it was damaged in combat.");
                  return -1;
            }
      }

/*
 *  Return the time of initial ground contact (or minus one).
 */

      if (tm != 2.0 || tn != 2.0) {
            c->flags |= FL_GND_CONTACT;
            if (tn < tm) {
                  tm = tn;
            }

            if (tm < 0.0) {
                  return -1.0;
            }

#ifdef DEBUG1
            contactSg->x = Sg.x + tm * deltaT * v.x;
            contactSg->y = Sg.y + tm * deltaT * v.y;
            contactSg->z = Sg.z + tm * deltaT * v.z - METERStoFEET(local_z);

            printf("Ground Contact, time t + %.5f sec\n", tm * deltaT);
            cosTheta = cos(-c->curPitch);
            sinTheta = sin(-c->curPitch);
            cm = -(contactSg->z + p->rm.x * sinTheta) / cosTheta -
                  p->Gm - p->rm.z;
            cn = -(contactSg->z + p->rn.x * sinTheta) / cosTheta -
                  p->Gn - p->rn.z;
            printf("cm/cn cmMax/cnMax %.4f %.4f %.4f %.4f\n\n",
                     cm, cn, p->cmMax, p->cnMax);
#endif
            return tm * deltaT;
      }
      else
            return -1.0;

}

/*
 *  Manage pitch and translation while we're on the ground.
 */

static VPoint zeroPt =
{0, 0, 0};

int
groundDynamics(craft * c, double startT, double CL,
                     double CD, double CM, double w, double qS)
{

      register double sinTheta, cosTheta;
      register craftType *p;
      double    theta, theta_dot, dT, cm, cn, cm_dot, cn_dot, m;
      double    t, next_t, muStatic;
      double    muKinetic, Mu, nMu;
      double    lift, drag, pitch_moment, angle;
      double    v, sinAlphaP, cosAlphaP;
      VPoint    F, M, mt, Sg, Cg, Fm, Fn, FnMu, FmMu, r, tmpPt, Ftot;
      VMatrix   turn, mtx;
      double    theta_damp;
      int       done = 0, niter = 0;
      double    smag;
      double    local_z = 0.0;

      t = 0.0;
      Fm.x = Fm.y = Fm.z = 0.0;
      Fn.x = Fn.y = Fn.z = 0.0;
      Ftot.x = Ftot.y = Ftot.z = 0.0;

      m = w / earth_g;

      dT = 0.002;

      p = c->cinfo;

      theta_damp = 0.25 * p->wingS * c->rho * p->c * p->c * p->Cmq;

/*
 *  Grab initial conditions
 *
 *  We restrict the degrees of freedom here to x and z translations and
 *  pitch rotations.  Yawing induced by nosewheel steering is controlled
 *  by some special code at the end of this procedure.
 *
 *  Note that the pitch of the aircraft is the value theta, with a positive
 *  value representing a downward pitch.
 */

/*
 *  First, change the velocity into a two-dimensional vector.
 */

      v = mag(c->Cg);
      Cg.z = c->Cg.z;
      Cg.y = 0.0;
      Cg.x = sqrt(v * v - Cg.z * Cg.z);

      Sg.x = Sg.y = 0.0;
#ifdef FLATWORLD
      Sg.z = c->Sg.z;
#else
      local_z = localAltitude(&c->Sg, &c->w);
      Sg.z = -METERStoFEET(c->w.z - local_z);
      c->localTerrain_feet = METERStoFEET ( local_z );
#endif
      theta = -c->curPitch;
      theta_dot = -c->q;

#ifdef DEBUG
      sinTheta = sin(theta);
      cosTheta = cos(theta);
      cm = -(Sg.z + p->rm.x * sinTheta) / cosTheta - p->Gm - p->rm.z;
      cn = -(Sg.z + p->rn.x * sinTheta) / cosTheta - p->Gn - p->rn.z;
      printf("start of groundDynamics\n");
      printf("time    z     theta     cm      cn        Cg.x\n");
      printf("%.3f  %.3f  %.2f  %f  %f   %f ft/sec\n", t, -Sg.z,
               theta * 180.0 / M_PI, cn, cm, Cg.x);
#endif

#ifdef notdef
      printf("theta = %f, theta_dot = % f\n ", theta * 180.0 / M_PI,
               theta_dot);
#endif

/*
 *  Since the time interval is very short, we'll treat the aero forces
 *  as constants.
 */

      lift = CL * qS;
      drag = CD * qS;
      pitch_moment = CM * (c->alpha + elevatorSetting(c, qS, w) *
                                     p->effElevator) * p->c * qS;

      if (c->flags & FL_BRAKES) {
            muStatic = p->muBStatic;
            muKinetic = p->muBKinetic;
      }
      else {
            muStatic = p->muStatic;
            muKinetic = p->muKinetic;
      }

/*
 *  Since lift and drag are constants in this model, well make their x and z
 *  force components constant as well.
 */

      sinAlphaP = sin(c->alpha + theta);
      cosAlphaP = cos(c->alpha + theta);

      for (t = startT; !done; t = next_t) {

            next_t = t + dT;
            if (next_t >= deltaT) {
                  done = 1;
                  dT = deltaT - t;
            }

            sinTheta = sin(theta);
            cosTheta = cos(theta);

/*
 *  cm and cn are the current strut positions (i.e. how compressed an oleo
 *  currently is). Zero corresponds to a fully compressed oleo.  Positive
 *  values mean that the oleo is extended by that amount.  Units are feet.
 */

            cm = -(Sg.z + p->rm.x * sinTheta) / cosTheta - p->Gm - p->rm.z;
            cn = -(Sg.z + p->rn.x * sinTheta) / cosTheta - p->Gn - p->rn.z;

/*
 *  cm_dot and cn_dot are the instantaneous strut motion rates.  They are used
 *  to compute the damping forces in each strut.
 */
            cm_dot = -(p->rm.x * theta_dot + Cg.z / cosTheta +
                           ((Sg.z + p->rm.x * sinTheta) * sinTheta * theta_dot) /
                           (cosTheta * cosTheta));

            cn_dot = -(p->rn.x * theta_dot + Cg.z / cosTheta +
                           ((Sg.z + p->rn.x * sinTheta) * sinTheta * theta_dot) /
                           (cosTheta * cosTheta));

#ifdef experimental
            cm_dot = -(cosTheta * Cg.z + theta_dot * (Sg.z * sinTheta + p->rm.x)) /
                  (cosTheta * cosTheta);

            cn_dot = -(cosTheta * Cg.z + theta_dot * (Sg.z * sinTheta + p->rn.x)) /
                  (cosTheta * cosTheta);
#endif

/*
 *  If cm or cn ever go negative, then that oleo -- and strut -- are considered
 *  to have collaped.
 */

            if (cm < 0.0) {
#ifdef DEBUG
                  printf ("main gear smash: %f\n", cm);
#endif
                  return 1;
            }

            if (cn < 0.0) {
#ifdef DEBUG
                  printf ("nose gear smash: %f\n", cn);
#endif
                  return 1;
            }

/* 
 *  Compute the force at each landing gear strut location.  Note that damping
 *  only occurs "on the downstroke".  That's because the wheel isn't physically
 *  attached to the ground.
 */

            if (cm < p->cmMax) {
                  Fm.z = -(p->Km * (p->cmMax - cm));
                  if (cm_dot < 0.0)
                        Fm.z += p->Dm * cm_dot;
            }
            else
                  Fm.z = 0.0;

            if (cn < p->cnMax) {
                  Fn.z = -(p->Kn * (p->cnMax - cn));
                  if (cn_dot < 0.0)
                        Fn.z += p->Dn * cn_dot;
                  if (cn < 0.1)
                        /* add resistance in the last 0.1 feet */
                        Fn.z += -100 * (p->Kn * (0.1 - cn));
            }
            else
                  Fn.z = 0.0;

/*
 *  Add moments contributed by oleo compression and oleo movement
 */

            VCrossProd(&Fm, &p->rm, &M);

            VCrossProd(&Fn, &p->rn, &mt);
            M.x += mt.x;
            M.y += mt.y;
            M.z += mt.z;

/*
 *  Add moments contributed by tire friction (we'll use rm/rn for
 *  the sake of speed here instead of calculating a more accurate
 *  tire / ground contact position.  Note that FmMu and FnMu are forces
 *  relative to the body axes, not world axes.
 */

            if (fabs(Cg.x) < 0.5) {
                  nMu = p->muStatic;
                  Mu = muStatic;
            }
            else {
                  nMu = p->muKinetic;
                  Mu = muKinetic;
            }

            if (Cg.x < 0.0)
                  Mu = -Mu;
            else if (Cg.x == 0.0)
                  Mu = 0.0;

            if (cm < p->cmMax) {
                  FmMu.x = Fm.z * cosTheta * cosTheta * Mu;
                  FmMu.y = 0.0;
                  FmMu.z = Fm.z * cosTheta * sinTheta * Mu;
                  VCrossProd(&FmMu, &p->rm, &mt);
                  M.x += mt.x;
                  M.y += mt.y;
                  M.z += mt.z;
            }

            if (cn < p->cnMax) {
                  FnMu.x = Fn.z * cosTheta * cosTheta * nMu;
                  FnMu.y = 0.0;
                  FnMu.z = Fn.z * cosTheta * sinTheta * nMu;
                  VCrossProd(&FnMu, &p->rn, &mt);
                  M.x += mt.x;
                  M.y += mt.y;
                  M.z += mt.z;
            }

            M.y -= pitch_moment - theta_damp * v * theta_dot;

/*
 *  Sum forces in (simplified) world axes
 */

            F.x = -(Fm.z + Fn.z) * sinTheta;
            F.x += (Fm.z + Fn.z) * cosTheta * Mu;
            F.x += cosTheta * c->curThrust;
            F.x -= sinAlphaP * lift;
            F.x -= cosAlphaP * drag;
            F.y = 0.0;
            F.z = w + (Fm.z + Fn.z) * cosTheta;
            F.z += sinTheta * c->curThrust;
            F.z -= cosAlphaP * lift;
            F.z -= sinAlphaP * drag;

            Ftot.x += F.x;
            Ftot.y += F.y;
            Ftot.z += F.z;
            niter++;
/*
 *  Update position and theta
 */

            Sg.x += Cg.x * dT + 0.5 * F.x / m * dT * dT;
            Sg.y += Cg.y * dT + 0.5 * F.y / m * dT * dT;
            Sg.z += Cg.z * dT + 0.5 * F.z / m * dT * dT;

            Cg.x += F.x / m * dT;
            Cg.y += F.y / m * dT;
            Cg.z += F.z / m * dT;

            theta += theta_dot * dT + 0.5 * M.y / p->I.m[1][1] * dT * dT;
            theta_dot += M.y / p->I.m[1][1] * dT;

/*
 *  Well, okay, it's time for a hack.  To simulate dragging the tail, we'll
 *  constrain theta to be less than 20 degrees (for now).  The right way to
 *  do this would be to determine if the tail contact point is positive and if
 *  it is, then limit theta.
 */

            if (theta < DEGtoRAD(-20.0)) {
                  theta = DEGtoRAD(-20.0);
                  theta_dot = 0.0;
            }

#ifdef DEBUG
            if (debug) {
                  printf("%.3f  %.3f  %.2f  %f  %f", t + dT, -Sg.z,
                           RADtoDEG(theta), cm, cn);
                  printf("  %f ft/sec\n", Cg.x);
            }
#endif

      }

      Sg.x = FEETtoMETERS(Sg.x);
      Sg.y = FEETtoMETERS(Sg.y);
      Sg.z = FEETtoMETERS(Sg.z);

/*
 *  Well, now we've calculated how far we've moved relative to our starting
 *  point (Sg.x), what our CG's new altitude is (Sg.z), and our ending pitch
 *  (theta).  We'll use this information as an input to nose-wheel steering
 *  control.
 */

      c->curNWDef = - c->Sa * c->cinfo->maxNWDef * 0.3;

      if (c->curNWDef != 0.0 && v < p->maxNWS && cn < p->cnMax) {

            tmpPt.x = c->cinfo->gearD2;
            tmpPt.y = c->cinfo->gearD1 / tan(c->curNWDef);
            tmpPt.z = 0.0;
            angle = Sg.x / tmpPt.y;

/*
 *  Nose wheel steering mode.
 *  Relocate the aircraft and its trihedral (this code assumes that the
 *  plane is rolling on a flat surface (i.e. z is constant).
 */

            VTransform(&tmpPt, &(c->trihedral), &r);

            VIdentMatrix(&turn);
#ifdef FLAT_WORLD
            turn.m[0][3] = -c->Sg.x - r.x;
            turn.m[1][3] = -c->Sg.y - r.y;
            turn.m[2][3] = -c->Sg.z;
            VRotate(&turn, ZRotation, angle);
            turn.m[0][3] = turn.m[0][3] + c->Sg.x + r.x;
            turn.m[1][3] = turn.m[1][3] + c->Sg.y + r.y;
            turn.m[2][3] = turn.m[2][3] + c->Sg.z;
            VTransform(&(c->Sg), &turn, &tmpPt);
            c->Sg = tmpPt;
#else
            turn.m[0][3] = -r.x;
            turn.m[1][3] = -r.y;
            turn.m[2][3] = -METERStoFEET(c->w.z);
            VRotate(&turn, ZRotation, angle);
            turn.m[0][3] = turn.m[0][3] + r.x;
            turn.m[1][3] = turn.m[1][3] + r.y;
            turn.m[2][3] = turn.m[2][3] + METERStoFEET(c->w.z);
            VTransform(&zeroPt, &turn, &tmpPt);
            smag = sqrt(tmpPt.x * tmpPt.x + tmpPt.y * tmpPt.y);
            DISUpdateWorldCoordinates(&c->w, tmpPt.x / smag, tmpPt.y / smag,
                                                  smag);
            c->w.z = -FEETtoMETERS(Sg.z);
#endif

            VIdentMatrix(&turn);
            VRotate(&turn, ZRotation, angle);
            mtx = c->trihedral;
            VMatrixMultByRank(&mtx, &turn, &(c->trihedral), 3);
            VTransform(&(c->Cg), &turn, &tmpPt);
            c->Cg = tmpPt;
            c->r = -angle / deltaT;

            euler(c);

            VSetPoint(tmpPt, 1.0, 0.0, 0.0);
            VTransform(&tmpPt, &(c->trihedral), &r);
            smag = sqrt(r.x * r.x + r.y * r.y);

      }
      else {
            VSetPoint(tmpPt, 1.0, 0.0, 0.0);
            VTransform(&tmpPt, &(c->trihedral), &r);
#ifdef FLAT_WORLD
            c->Sg.x += Sg.x * r.x;
            c->Sg.y += Sg.x * r.y;  /* yes, I really want Sg.x */
#else
            smag = sqrt(r.x * r.x + r.y * r.y);
            DISUpdateWorldCoordinates(&c->w, r.x / smag, r.y / smag, Sg.x);
#endif
            c->r = 0.0;
      }

      c->Cg.x = Cg.x * r.x / smag;
      c->Cg.y = Cg.x * r.y / smag;
      c->Cg.z = Cg.z;
      c->groundCgx = Cg.x;

#ifdef FLAT_WORLD
      c->Sg.z = Sg.z;
#else
      c->w.z = -Sg.z + local_z;
#endif
      c->curPitch = -theta;
      c->curRoll = 0.0;
      c->p = 0.0;                         /* zero roll speed when on ground */
      c->q = -theta_dot;

      buildEulerMatrix(c->curRoll, c->curPitch, c->curHeading,
                               &(c->trihedral));

/*
 *  Update (averaged) acceleration and the user perceived G-Forces
 *  Keep in mind that F (and Ftot) are forces in an idealized frame that is
 *  the NED frame rotated to the heading of the aircraft.
 */

      c->linAcc.x = (Ftot.x * cosTheta - Ftot.z * sinTheta) / (niter * m);
      c->linAcc.y = 0.0;
      c->linAcc.z = -(Ftot.x * sinTheta + Ftot.z * cosTheta) / (niter * m);

      c->G.z = c->linAcc.x / earth_g;
      c->G.y = c->linAcc.y / earth_g;
      c->G.z = c->linAcc.z / earth_g - 1.0;

/*
 *  Is the plane still in contact with the ground?
 */

      if ((cm >= p->cmMax) && (cn >= p->cnMax)) {
            cosTheta = cos(theta);
            sinTheta = sin(theta);
            cm = -(Sg.z + p->rm.x * sinTheta) / cosTheta - p->Gm - p->rm.z;
            cn = -(Sg.z + p->rn.x * sinTheta) / cosTheta - p->Gn - p->rn.z;
            if ((cm >= p->cmMax) && (cn >= p->cnMax)) {
                  c->flags &= ~FL_GND_CONTACT;
                  c->groundCgx = 0.0;
            }
      }

#ifdef DEBUG1
      if (debug) {
            printf("Sg.z = %.3f  theta=%.2f  alpha= %.2f  cm/cn %f  %f", -Sg.z,
                     RADtoDEG(theta), RADtoDEG(c->alpha), cm, cn);
            printf("  %f ft/sec\n", Cg.x);
            printf("cm-dot/cn-dot = %0.2f  %0.2f\n", cm_dot, cn_dot);
      }
#endif

      return 0;

}

Generated by  Doxygen 1.6.0   Back to index