//------------------------------------------------------------------------------
// stepOwnshipPlayer() -- Step to the next local player
//------------------------------------------------------------------------------
void SimStation::stepOwnshipPlayer()
{
   Basic::PairStream* pl = getSimulation()->getPlayers();
   if (pl != nullptr) {

      Simulation::Player* f = nullptr;
      Simulation::Player* n = nullptr;
      bool found = false;

      // Find the next player
      Basic::List::Item* item = pl->getFirstItem();
      while (item != nullptr) {
         Basic::Pair* pair = static_cast<Basic::Pair*>(item->getValue());
         if (pair != nullptr) {
            Simulation::Player* ip = static_cast<Simulation::Player*>(pair->object());
            if ( ip->isMode(Simulation::Player::ACTIVE) &&
               ip->isLocalPlayer() &&
               ip->isClassType(typeid(Simulation::AirVehicle))
               ) {
                  if (f == nullptr) { f = ip; }  // Remember the first
                  if (found)        { n = ip; ; break; }
                  if (ip == getOwnship()) found = true;
            }
         }
         item = item->getNext();
      }
      if (found && n == nullptr) n = f;
      if (n != nullptr) setOwnshipPlayer(n);

      pl->unref();
    }
}

// Dynamics model interface
bool LaeroModel::setCommandedHeadingD(const double h, const double hDps, const double maxBank)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );

   bool ok = (pPlr != nullptr);
   if (ok) {

      //----------------------------------------------------
      // define local constants
      //----------------------------------------------------
      const double MAX_BANK_RAD = maxBank * Basic::Angle::D2RCC;
      //const double TAU = 2.0;  // time constant [sec]
      const double TAU = 1.0;  // time constant [sec]

      //-------------------------------------------------------
      // get current data
      //-------------------------------------------------------
      double velMps        = pPlr->getTotalVelocity();
      double hdgDeg        = pPlr->getHeadingD();
      double hdgErrDeg     = Basic::Angle::aepcdDeg(h - hdgDeg);
      double hdgErrAbsDeg  = std::fabs(hdgErrDeg);

      //-------------------------------------------------------
      // get absolute heading rate of change (hdgDotAbsDps)
      //-------------------------------------------------------
      double hdgDotMaxAbsRps = Eaagles::ETHGM * std::tan(MAX_BANK_RAD) / velMps;
      double hdgDotMaxAbsDps = hdgDotMaxAbsRps * Basic::Angle::R2DCC;

      double hdgDotAbsDps = hDps;
      if (hdgDotAbsDps > hdgDotMaxAbsDps) {
         hdgDotAbsDps = hdgDotMaxAbsDps;
      }

      double hdgErrBrkAbsDeg = TAU * hdgDotAbsDps;
      if (hdgErrAbsDeg < hdgErrBrkAbsDeg) {
         hdgDotAbsDps = hdgErrAbsDeg / TAU;
      }

      //-------------------------------------------------------
      // define direction of heading rate of change (hdgDotDps)
      //-------------------------------------------------------
      double hdgDotDps = sign(hdgErrDeg) * hdgDotAbsDps;
      psiDot = hdgDotDps * Basic::Angle::D2RCC;

      //-------------------------------------------------------
      // define bank angle as a function of turn rate
      //-------------------------------------------------------
      double phiCmdDeg = std::atan2(psiDot * velMps, Eaagles::ETHGM) * Basic::Angle::R2DCC;
      ok = flyPhi(phiCmdDeg);
   }

   return ok;
}

//----------------------------------------------------------
// reset() -- sets up our initial flying values
//----------------------------------------------------------
void LaeroModel::reset()
{
   BaseClass::reset();

   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   if (pPlr != nullptr) {
      LCreal initVel = pPlr->getInitVelocity();
      u = initVel * Basic::Distance::NM2M / Basic::Time::H2S;
   }
}

// setCommandedVelocityKts() - also can limit velocity rate of acceleration
bool LaeroModel::setCommandedVelocityKts(const double v, const double vNps)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   bool ok = (pPlr != nullptr);
   if (ok) {

      //-------------------------------------------------------
      // define local constants
      //-------------------------------------------------------
      const double KTS2MPS = Basic::Distance::NM2M / Basic::Time::H2S;

      //-------------------------------------------------------
      // convert argument units (deg -> rad)
      //-------------------------------------------------------
      double velCmdMps     = v * KTS2MPS;
      double velDotCmdMps2 = vNps * KTS2MPS;

      //-------------------------------------------------------
      // current vel error (rad)
      //-------------------------------------------------------
      double velMps    = pPlr->getTotalVelocityKts() * KTS2MPS;
      double velErrMps = velCmdMps - velMps;

      //-------------------------------------------------------
      // vel error break point (rad)
      //-------------------------------------------------------
      const double TAU = 1.0;  // time constant [sec]
      double velErrBrkMps = velDotCmdMps2 * TAU;

      //-------------------------------------------------------
      // control signal for commanded vel (rps)
      //-------------------------------------------------------
      double velDotMps2 = sign(velErrMps) * velDotCmdMps2;
      if (std::abs(velErrMps) < velErrBrkMps) {
         velDotMps2 = (velErrMps / velErrBrkMps) * velDotCmdMps2;
      }

      //-------------------------------------------------------
      // assign result to velocity control
      //-------------------------------------------------------
      uDot = velDotMps2;
   }

   return true;
}

// Dynamics model interface - all input values in meters
bool LaeroModel::setCommandedAltitude(const double a, const double aMps, const double maxPitch)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );

   bool ok = (pPlr != nullptr);
   if (ok) {

      //-------------------------------------------------------
      // define local constants
      //-------------------------------------------------------
      const double TAU            = 4.0;  // time constant [sec]

      //-------------------------------------------------------
      // get current alt error (mtr)
      //-------------------------------------------------------
      double altMtr     = pPlr->getAltitude();
      double altErrMtr = a - altMtr;

      //-------------------------------------------------------
      // get alt error break point (mtr)
      //-------------------------------------------------------
      double altDotCmdMps = aMps;
      double altErrBrkMtr = altDotCmdMps * TAU;

      //-------------------------------------------------------
      // get commanded altDot (mps)
      //-------------------------------------------------------
      double altDotMps = sign(altErrMtr) * altDotCmdMps;
      if (std::abs(altErrMtr) < altErrBrkMtr) {
         altDotMps = altErrMtr * (altDotCmdMps / altErrBrkMtr);
      }

      //-------------------------------------------------------
      // assign result to altitude control
      //-------------------------------------------------------
      double thtCmdDeg = (altDotMps / u) * Basic::Angle::R2DCC;
      // SLS - TO DO: Limit commanded pitch to max pitch angle as well.
      ok = flyTht(thtCmdDeg);
   }

   return ok;
}

bool LaeroModel::flyPsi(const double psiCmdDeg, const double psiDotCmdDps)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   bool ok = (pPlr != nullptr);
   if (ok) {

      //-------------------------------------------------------
      // convert argument units (deg -> rad)
      //-------------------------------------------------------
      double psiCmdRad    = psiCmdDeg * Basic::Angle::D2RCC;
      double psiDotCmdRps = psiDotCmdDps * Basic::Angle::D2RCC;

      //-------------------------------------------------------
      // current psi error (rad)
      //-------------------------------------------------------
      double psiRad    = pPlr->getHeadingR();
      double psiErrRad = psiCmdRad - psiRad;

      //-------------------------------------------------------
      // psi error break point (rad)
      //-------------------------------------------------------
      const double TAU = 1.0;  // time constant [sec]
      double psiErrBrkRad = psiDotCmdRps * TAU;

      //-------------------------------------------------------
      // control signal for commanded psi (rps)
      //-------------------------------------------------------
      double psiDotRps = sign(psiErrRad) * psiDotCmdRps;
      if (std::abs(psiErrRad) < psiErrBrkRad) {
         psiDotRps = (psiErrRad / psiErrBrkRad) * psiDotCmdRps;
      }

      //-------------------------------------------------------
      // assign result to azimuth control
      //-------------------------------------------------------
      psiDot = psiDotRps;
   }

   return ok;
}

bool LaeroModel::flyTht(const double thtCmdDeg, const double thtDotCmdDps)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   bool ok = (pPlr != nullptr);
   if (ok) {

      //-------------------------------------------------------
      // convert argument units (deg -> rad)
      //-------------------------------------------------------
      double thtCmdRad    = thtCmdDeg * Basic::Angle::D2RCC;
      double thtDotCmdRps = thtDotCmdDps * Basic::Angle::D2RCC;

      //-------------------------------------------------------
      // current tht error (rad)
      //-------------------------------------------------------
      double thtRad    = pPlr->getPitchR();
      double thtErrRad = thtCmdRad - thtRad;

      //-------------------------------------------------------
      // tht error break point (rad)
      //-------------------------------------------------------
      const double TAU = 1.0;  // time constant [sec]
      double thtErrBrkRad = thtDotCmdRps * TAU;

      //-------------------------------------------------------
      // control signal for commanded tht (rps)
      //-------------------------------------------------------
      double thtDotRps = sign(thtErrRad) * thtDotCmdRps;
      if (std::abs(thtErrRad) < thtErrBrkRad) {
         thtDotRps = (thtErrRad / thtErrBrkRad) * thtDotCmdRps;
      }

      //-------------------------------------------------------
      // assign result to pitch control
      //-------------------------------------------------------
      thtDot = thtDotRps;
   }

   return ok;
}

//------------------------------------------------------------------------------
// reset() -- 
//------------------------------------------------------------------------------
void JSBSimModel::reset()
{
    BaseClass::reset();

    pitchTrimPos  = (LCreal)0.0;
    pitchTrimRate = (LCreal)0.1;
    pitchTrimSw   = (LCreal)0.0;
    rollTrimPos   = (LCreal)0.0;
    rollTrimRate  = (LCreal)0.1;
    rollTrimSw    = (LCreal)0.0;

    // Get our Player (must have one!)
    Simulation::Player* p = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
    if (p == 0) return;

    // must have strings set
    if (rootDir == 0 || model == 0) return;

    // Must also have the JSBSim object
    if (fdmex == 0) {
        // must have a JSBSim property manager
        if (propMgr == 0) {
            propMgr = new JSBSim::FGPropertyManager();
        }
        fdmex = new JSBSim::FGFDMExec(propMgr);

        std::string RootDir(rootDir->getString());
        fdmex->SetAircraftPath(RootDir + "aircraft");
        fdmex->SetEnginePath(RootDir + "engine");
        fdmex->SetSystemsPath(RootDir + "systems"); // JSBSim-1.0 or after only

        fdmex->LoadModel(model->getString());
        JSBSim::FGPropertyManager* propMgr = fdmex->GetPropertyManager();
        if (propMgr != 0) {
            hasHeadingHold = propMgr->HasNode("ap/heading_hold") && propMgr->HasNode("ap/heading_setpoint");
            hasVelocityHold = propMgr->HasNode("ap/airspeed_hold") && propMgr->HasNode("ap/airspeed_setpoint");
            hasAltitudeHold = propMgr->HasNode("ap/altitude_hold") && propMgr->HasNode("ap/altitude_setpoint");
#if 0
            // CGB this isn't working for some reason. I set the values directly in "dynamics" for now.
            if (hasHeadingHold) {
                propMgr->Tie("ap/heading_hold", this, &JSBSimModel::isHeadingHoldOn);
                propMgr->Tie("ap/heading_setpoint", this, &JSBSimModel::getCommandedHeadingD);
            }
            if (hasVelocityHold) {
                propMgr->Tie("ap/airspeed_hold", this, &JSBSimModel::isVelocityHoldOn);
                propMgr->Tie("ap/airspeed_setpoint", this, &JSBSimModel::getCommandedVelocityKts);
            }
            if (hasAltitudeHold) {
                propMgr->Tie("ap/altitude_hold", this, &JSBSimModel::isAltitudeHoldOn);
                propMgr->Tie("ap/altitude_setpoint", this, &JSBSimModel::getCommandedAltitude * Basic::Distance::M2FT);
            }
#endif
        }
    }
    
#if 0
    // CGB TBD
    reset = 0;
    freeze = 0;
#endif
    JSBSim::FGInitialCondition* fgic = fdmex->GetIC();
    if (fgic == 0) return;

    fgic->SetAltitudeASLFtIC(Basic::Distance::M2FT * p->getAltitude());

#if 0
    fgic->SetTrueHeadingDegIC(Basic::Angle::R2DCC * p->getHeading());
    fgic->SetRollAngleDegIC(Basic::Angle::R2DCC * p->getRoll());
    fgic->SetPitchAngleDegIC(Basic::Angle::R2DCC * p->getPitch());
#else
    fgic->SetPsiDegIC(Basic::Angle::R2DCC * p->getHeading());
    fgic->SetPhiDegIC(Basic::Angle::R2DCC * p->getRoll());
    fgic->SetThetaDegIC(Basic::Angle::R2DCC * p->getPitch());
#endif
    fgic->SetVtrueKtsIC(Basic::Distance::M2NM * p->getTotalVelocity() * 3600.0f);
    fgic->SetLatitudeDegIC(p->getInitLatitude());
    fgic->SetLongitudeDegIC(p->getInitLongitude());

    JSBSim::FGPropulsion* Propulsion = fdmex->GetPropulsion();
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (Propulsion != 0 && FCS != 0) {
        Propulsion->SetMagnetos(3);
        for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) {
            FCS->SetMixtureCmd(i, 1.0);
            FCS->SetThrottleCmd(i, 1.0);
            FCS->SetPropAdvanceCmd(i, 1.0);
            FCS->SetMixturePos(i, 1.0);
            FCS->SetThrottlePos(i, 1.0);
            FCS->SetPropAdvance(i, 1.0);
            JSBSim::FGEngine* eng = Propulsion->GetEngine(i);
            eng->SetRunning(true);
            JSBSim::FGThruster* thruster = eng->GetThruster();
            thruster->SetRPM(1000.0);
        }
        Propulsion->SetFuelFreeze(p->isFuelFrozen());
        Propulsion->InitRunning(-1);     // -1 refers to "All Engines"
        Propulsion->GetSteadyState();
//.........这里部分代码省略.........

//------------------------------------------------------------------------------
// dynamics() -- update player's vehicle dynamics
//------------------------------------------------------------------------------
void JSBSimModel::dynamics(const LCreal dt)
{

    // Get our Player (must have one!)
    Simulation::Player* p = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
    if (p == 0) return;

    if (fdmex == 0) return;

    JSBSim::FGPropagate* Propagate = fdmex->GetPropagate();
    if (Propagate == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;
    JSBSim::FGAccelerations* Accelerations = fdmex->GetAccelerations();
    if (Accelerations == 0) return;

    pitchTrimPos += pitchTrimRate * pitchTrimSw * dt;
    if (pitchTrimPos > 1.0) {
        pitchTrimPos = 1.0;
    } else if (pitchTrimPos < -1.0) {
        pitchTrimPos = -1.0;
    }
    FCS->SetPitchTrimCmd(pitchTrimPos);

    rollTrimPos += rollTrimRate * rollTrimSw * dt;
    if (rollTrimPos > 1.0) {
        rollTrimPos = 1.0;
    } else if (rollTrimPos < -1.0) {
        rollTrimPos = -1.0;
    }
    FCS->SetRollTrimCmd(rollTrimPos);

    fdmex->Setdt(dt);
    // ---
    // Pass flags & Data
    // ---
#if 0
    // CGB TBD
    if (isFrozen() || dt == 0) freeze = -1;
    else freeze = 0;

    if (isPositionFrozen()) posFrz = -1;
    else posFrz = 0;

    if (isAltitudeFrozen()) altFrz = -1;
    else altFrz = 0;

    if (isFuelFrozen()) fuelFrz = -1;
    else fuelFrz = 0;

    rw_elev = getTerrainElevationFt();
#endif

    // ---
    // Run the model
    // ---
    fdmex->Run();     //loop JSBSim once w/o integrating

    // ---
    // Set values for Player & AirVehicle interfaces
    //    (Note: Player::dynamics() computes the new position)
    // ---
    p->setAltitude(Basic::Distance::FT2M * Propagate->GetAltitudeASL(), true);
    p->setVelocity((LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eNorth)), (LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eEast)), (LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eDown)));
    p->setVelocityBody((LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(1)), (LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(2)), (LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(3)));
//    LCreal accX = Basic::Distance::FT2M * Propagate->GetUVWdot(1);
//    LCreal accY = Basic::Distance::FT2M * Propagate->GetUVWdot(2);
//    LCreal accZ = Basic::Distance::FT2M * Propagate->GetUVWdot(3);
    const JSBSim::FGMatrix33& Tb2l = Propagate->GetTb2l();
    const JSBSim::FGColumnVector3& vUVWdot = Accelerations->GetUVWdot();

    p->setEulerAngles((LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::ePhi)), (LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::eTht)), (LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::ePsi)));
    p->setAngularVelocities((LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eP)), (LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eQ)), (LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eR)));

    JSBSim::FGColumnVector3 vVeldot = Tb2l * vUVWdot;
    p->setAcceleration((LCreal)(Basic::Distance::FT2M * vVeldot(1)), (LCreal)(Basic::Distance::FT2M * vVeldot(2)), (LCreal)(Basic::Distance::FT2M * vVeldot(3)));

    //std::printf("(%6.1f, %6.1f, %6.1f)   vel=%8.1f   alt=%8.1f alt2=%8.1f\n", acData->phi, acData->theta, acData->psi, acData->vp, acData->hp, (M2FT*getAltitude()) );
    //std::printf("f=%6.1f p=%6.1f, qa=%6.1f, a=%6.1f, g=%6.1f\n", hotasIO->pitchForce, acData->theta, acData->qa, acData->alpha, acData->gamma );

        //{
        // std::cout << "JSBSim: ---------------------------------" << std::endl;
        // osg::Vec4 fq;
        // fq.set(acData->e1, acData->e2, acData->e4, acData->e4);
        // osg::Matrix m2;
        // m2.set(
        //    acData->l1, acData->l2, acData->l3, 0,
        //    acData->m1, acData->m2, acData->m3, 0,
        //    acData->n1, acData->n2, acData->n3, 0,
        //    0,          0,          0,          1
        //    );
        // std::printf("Eaagles*EA: (%6.1f, %6.1f, %6.1f)\n", getRollD(), getPitchD(), getHeadingD());
        // osg::Matrix m0 = getRotationalMatrix();
        // osg::Quat q0 = getQuaternions();
        // setRotationalMatrix(m2);
        // //setQuaternions(fq);
        // osg::Quat eq = getQuaternions();
//.........这里部分代码省略.........

// ----------------------------------------------------------------------------
// buttonEvent() - called when a button is pressed
// ----------------------------------------------------------------------------
void MapDisplay::buttonEvent(const int b)
{
   // cmdRange up, down
   MapPage* page = static_cast<MapPage*>(subpage());

   // cmdAirspeed, cmdAltitude, cmdHeading up, down
   Simulation::Player* pA = getOwnship();
   Simulation::Autopilot* ap = 0;
   if (pA != 0) {
      ap = static_cast<Simulation::Autopilot*>(pA->getPilot());
   }
   if (page != 0 && ap != 0) {
      if (b == DEC_RANGE) {
         if (page->getRange() > 5) {
            page->setRange(page->getRange() - 5);
         }
      }
      else if (b == INC_RANGE) {
         if (page->getRange() < 320) {
            page->setRange(page->getRange() + 5);
         }
      }
      else if (b == DEC_CMD_AS) {
         double cmdAirspeed = ap->getCommandedVelocityKts();
         if (cmdAirspeed > 100) {
            cmdAirspeed -= 10;
            ap->setCommandedVelocityKts(cmdAirspeed);
         }      
      }
      else if (b == INC_CMD_AS) {
         double cmdAirspeed = ap->getCommandedVelocityKts();
         if (cmdAirspeed < 400) {
            cmdAirspeed += 10;
            ap->setCommandedVelocityKts(cmdAirspeed);
         }
      }
      else if (b == DEC_CMD_ALT) {
         double cmdAltitude = ap->getCommandedAltitudeFt();
         if (cmdAltitude > 1000) {
            cmdAltitude -= 500;
            ap->setCommandedAltitudeFt(cmdAltitude);
         }      
      }
      else if (b == INC_CMD_ALT) {
         double cmdAltitude = ap->getCommandedAltitudeFt();
         if (cmdAltitude < 40000) {
            cmdAltitude += 500;
            ap->setCommandedAltitudeFt(cmdAltitude);
         }
      }
      else if (b == DEC_CMD_HDG) {
         double cmdHeading = ap->getCommandedHeadingD();
         cmdHeading -= 10;
         if (cmdHeading < -180.0) cmdHeading += 360;
         ap->setCommandedHeadingD(cmdHeading);
      }
      else if (b == INC_CMD_HDG) {
         double cmdHeading = ap->getCommandedHeadingD();
         cmdHeading += 10;
         if (cmdHeading > 180.0) cmdHeading -= 360;
         ap->setCommandedHeadingD(cmdHeading);
      }
      else if (b == INC_CMD_AS_NPS) {
         double maxAccel = ap->getMaxVelAcc();
         if (maxAccel < 20) maxAccel++;
         ap->setMaxVelAccNps(maxAccel);
      }
      else if (b == DEC_CMD_AS_NPS) {
         double maxAccel = ap->getMaxVelAcc();
         if (maxAccel > 1) maxAccel--;
         ap->setMaxVelAccNps(maxAccel);
      }
      // Climb rate in meters per second
      else if (b == INC_CMD_ALT_MPS) {
         double maxClimb = ap->getMaxClimbRate();
         if (maxClimb < 100) maxClimb += 5;
         ap->setMaxClimbRateMps(maxClimb);
      }
      else if (b == DEC_CMD_ALT_MPS) {
         double maxClimb = ap->getMaxClimbRate();
         if (maxClimb > 5) maxClimb -= 5;
         ap->setMaxClimbRateMps(maxClimb);
      }
      // Turn rate in degrees per second
      else if (b == INC_CMD_HDG_ROT) {
         double maxTR = ap->getMaxTurnRate();
         if (maxTR < 25) maxTR++;
         ap->setMaxTurnRateDps(maxTR);
      }
      else if (b == DEC_CMD_HDG_ROT) {
         double maxTR = ap->getMaxTurnRate();
         if (maxTR > 0) maxTR--;
         ap->setMaxTurnRateDps(maxTR);
      }
      // Max bank (degrees)
      else if (b == INC_CMD_HDG_BNK) {
         double maxBank = ap->getMaxBankAngle();
//.........这里部分代码省略.........

//------------------------------------------------------------------------------
// drawFunc()
//------------------------------------------------------------------------------
void Display::drawFunc()
{
   Simulation::Player* own = getOwnship();

   const Basic::Pair* pair = nullptr;
   if (own != nullptr) pair = own->getSensorByType(typeid(RealBeamRadar));

   const RealBeamRadar* rdr = nullptr;
   if (pair != nullptr) rdr = static_cast<const RealBeamRadar*>( pair->object() );

   const GLubyte* image = nullptr;  // The image pixels
   if (rdr != nullptr) image = rdr->getImage();

   if (image != nullptr) {

      // Image width  (number of columns)
      GLsizei imgWidth = rdr->getImageWidth();

      // Image height (number of rows)
      GLsizei imgHeight = rdr->getImageHeight();

      GLint pixelSize = rdr->getPixelSize();
      GLenum format = GL_RGB;
      if (pixelSize == 4) format = GL_RGBA;

      if (testTexture) {
         // ---
         // Draw using texture map
         // ---

         glEnable(GL_TEXTURE_2D);
         glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);

         if (texture == 0) {
            glGenTextures(1, &texture);
         }

         glBindTexture(GL_TEXTURE_2D, texture);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
         glPixelStorei(GL_UNPACK_ALIGNMENT, 1);

//         double start = getComputerTime();

         glTexImage2D(GL_TEXTURE_2D, 0, pixelSize, imgWidth, imgHeight, 0, format, GL_UNSIGNED_BYTE, image);

         // B SCAN
         glBegin(GL_POLYGON);

            glTexCoord2f( 1.0f, 1.0f );
            glVertex2f(  1.0f,  1.0f );

            glTexCoord2f( 0.0f, 1.0f );
            glVertex2f( 0.0f, 1.0f );

            glTexCoord2f( 0.0f, 0.0f );
            glVertex2f(  0.0f, 0.0f );

            glTexCoord2f( 1.0f, 0.0f );
            glVertex2f( 1.0f, 0.0f );

         glEnd();

         glDisable(GL_TEXTURE_2D);
//         double end = getComputerTime();
//         double dtime = (end - start);
         //std::cout << "glTexImage2D() dtime = " << dtime << std::endl;
      }
      else {
         // ---
         // Draw using glDrawPixels()
         // ---

         glRasterPos2f(0.0, 0.0);

         //double start = getComputerTime();

         glDrawPixels(imgWidth, imgHeight, format, GL_UNSIGNED_BYTE, image);

         //double end = getComputerTime();
         //double dtime = (end - start);
         //std::cout << "glDrawPixels() dtime = " << dtime << std::endl;
      }
   }
}

//------------------------------------------------------------------------------
// updateRAC -- update Robot Aircraft
//------------------------------------------------------------------------------
void RacModel::updateRAC(const LCreal dt)
{
   // Get our Player (must have one!)
   Simulation::Player* pp = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   if (pp == nullptr) return;

   // Acceleration of Gravity (M/S)
   LCreal g = ETHG * Basic::Distance::FT2M;

   // Set default commanded values
   if (cmdAltitude < -9000.0)
       cmdAltitude = static_cast<LCreal>(pp->getAltitudeM());
   if (cmdHeading < -9000.0)
       cmdHeading = static_cast<LCreal>(pp->getHeadingD());
   if (cmdVelocity < -9000.0)
       cmdVelocity = pp->getTotalVelocityKts();

   // ---
   // Compute delta altitude; commanded vertical velocity and
   // commanded flight path angle
   // ---

   // Max altitude rate 6000 ft /min converted to M/S
   double maxAltRate = (3000.0 / 60.0) * Basic::Distance::FT2M;

   // commanded vertical velocity is delta altitude limited to max rate
   double cmdAltRate = (cmdAltitude - pp->getAltitudeM());
   if (cmdAltRate > maxAltRate) cmdAltRate = maxAltRate;
   else if (cmdAltRate < -maxAltRate) cmdAltRate = -maxAltRate;

   // Compute commanded flight path angle (gamma)
   double cmdPitch = 0;
   LCreal vt = pp->getTotalVelocity();
   if (vt > 0) {
      cmdPitch = std::asin( cmdAltRate/vt );
   }

   // ---
   // Compute Max G
   // ---
   LCreal gmax = gMax;                     // Max g,s
   if(pp->getTotalVelocity() < vpMaxG) {
      gmax = 1.0f + (gMax - 1.0f) * (pp->getTotalVelocity() - vpMin) / (vpMaxG - vpMin);
   }

   // ---
   // Computer max turn rate, max/min pitch rates
   // ---
   LCreal ra_max = gmax * g / pp->getTotalVelocity();    // Turn rate base on vp and g,s (rad/sec)
   LCreal qa_max = ra_max;                           // Max pull up pitch rate (rad/sec)
   LCreal qa_min = -qa_max;                          // Max pushover pitch rate (rad/sec)
   if(gmax > 2.0) {
      // Max yaw rate (rad/sec)
      qa_min = -( 2.0f * g / pp->getTotalVelocity());
   }

   // ---
   // Get old angular values
   // ---
   const osg::Vec3 oldRates = pp->getAngularVelocities();
   //LCreal pa1 = oldRates[Simulation::Player::IROLL];
   LCreal qa1 = oldRates[Simulation::Player::IPITCH];
   LCreal ra1 = oldRates[Simulation::Player::IYAW];

   // ---
   // Find pitch rate and update pitch
   // ---
   double qa = Basic::Angle::aepcdRad(cmdPitch - pp->getPitchR()) * 0.1;
   if(qa > qa_max) qa = qa_max;
   if(qa < qa_min) qa = qa_min;

   // Find turn rate
   double ra = Basic::Angle::aepcdRad((cmdHeading  * Basic::Angle::D2RCC) - pp->getHeadingR()) * 0.1;
   if(ra > ra_max) ra = ra_max;
   if(ra < -ra_max) ra = -ra_max;

   // Damage
   double dd = pp->getDamage();
   if (dd > 0.5) {
      ra += (dd - 0.5) * ra_max;
      qa -= (dd - 0.5) * qa_max;
   }

   // Using Pitch rate, integrate pitch
   double newTheta = static_cast<LCreal>(pp->getPitch() + (qa + qa1) * dt / 2.0);

   // Use turn rate integrate heading
   double newPsi = static_cast<LCreal>(pp->getHeading() + (ra + ra1) * dt / 2.0);
   if(newPsi > 2.0*PI) newPsi -= 2.0*PI;
   if(newPsi < 0.0) newPsi += 2.0*PI;

   // Roll angle is proportional to max turn rate - filtered
   double pa = 0.0;
   double newPhi = 0.98 * pp->getRollR() + 0.02 * (ra / ra_max * (Basic::Angle::D2RCC * 60.0));

   // Find Acceleration
   double cmdVelMPS = cmdVelocity * (Basic::Distance::NM2M / 3600.0);
//.........这里部分代码省略.........

//------------------------------------------------------------------------------
// munitionDetonationMsgFactory() -- (Output) Munition detonation message factory
//------------------------------------------------------------------------------
bool Nib::munitionDetonationMsgFactory(const LCreal)
{
   // Dummy weapon?
   const Simulation::Weapon* ww = dynamic_cast<const Simulation::Weapon*>( getPlayer() );
   if (ww != 0) {
      if (ww->isDummy()) return true;
   }

    bool ok = true;
    //std::cout << "NetIO::munitionDetonationMsgFactory() HERE!!" << std::endl;

    // Get our NetIO
    NetIO* disIO = (NetIO*)(getNetIO());

    // If our NIB's player just detonated, then it must be a weapon!
    Simulation::Weapon* mPlayer = dynamic_cast<Simulation::Weapon*>(getPlayer());
    if (mPlayer == 0) return false;

    // Ok, we have the weapon, now get the firing and target players
    Simulation::Player* tPlayer = mPlayer->getTargetPlayer();
    Simulation::Player* fPlayer = mPlayer->getLaunchVehicle();
    if (fPlayer == 0) return false;

    // ---
    // PDU header
    // ---
    DetonationPDU pdu;
    pdu.header.protocolVersion = disIO->getVersion();
    pdu.header.PDUType = NetIO::PDU_DETONATION;
    pdu.header.protocolFamily = NetIO::PDU_FAMILY_WARFARE;
    pdu.header.length = sizeof(DetonationPDU);
    pdu.header.exerciseIdentifier = disIO->getExerciseID();
    pdu.header.timeStamp = disIO->timeStamp();
    pdu.header.status = 0;
    pdu.header.padding = 0;

    // ---
    // Set the PDU data with the firing (launcher) player's id
    // ---
    pdu.firingEntityID.ID = fPlayer->getID();
    pdu.firingEntityID.simulationID.siteIdentification = getSiteID();
    pdu.firingEntityID.simulationID.applicationIdentification = getApplicationID();

    // ---
    // Set the PDU data with the munition's ID
    // ---
    pdu.munitionID.ID = mPlayer->getID();
    pdu.munitionID.simulationID.siteIdentification = getSiteID();
    pdu.munitionID.simulationID.applicationIdentification = getApplicationID();

    // ---
    // Set the PDU data with the target's ID
    // ---
    {
         bool tOk = false;
         if (tPlayer != 0) {
            pdu.targetEntityID.ID = tPlayer->getID();
            if (tPlayer->isLocalPlayer()) {
               // Local player, use our site/app/exerc IDs
               pdu.targetEntityID.simulationID.siteIdentification = getSiteID();
               pdu.targetEntityID.simulationID.applicationIdentification = getApplicationID();
               tOk = true;
            }
            else {
               // Networked player, use its NIB's IDs
               const Nib* fNIB = dynamic_cast<const Nib*>( tPlayer->getNib() );
               if (fNIB != 0) {
                  pdu.targetEntityID.simulationID.siteIdentification = fNIB->getSiteID();
                  pdu.targetEntityID.simulationID.applicationIdentification = fNIB->getApplicationID();
                  tOk = true;
               }
            }
         }
         if (!tOk) {
            pdu.targetEntityID.ID = 0;
            pdu.targetEntityID.simulationID.siteIdentification = 0;
            pdu.targetEntityID.simulationID.applicationIdentification = 0;
         }
    }

    // ---
    // Event ID
    // ---
    pdu.eventID.simulationID.siteIdentification = getSiteID();
    pdu.eventID.simulationID.applicationIdentification = getApplicationID();
    pdu.eventID.eventNumber = mPlayer->getReleaseEventID();

    // ---
    // Location & Velocity
    // ---

    // World Coordinates
    osg::Vec3d geocPos = mPlayer->getGeocPosition();
    pdu.location.X_coord = geocPos[Basic::Nav::IX];
    pdu.location.Y_coord = geocPos[Basic::Nav::IY];
    pdu.location.Z_coord = geocPos[Basic::Nav::IZ];

//.........这里部分代码省略.........

//----------------------------------------------------------
// update4DofModel -- update equations of motion
//----------------------------------------------------------
void LaeroModel::update4DofModel(const LCreal dt)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );

   if (pPlr != nullptr) {

      //==============================================================
      // ROTATIONAL EOM
      //==============================================================

      //----------------------------------------------------
      // integrate Euler angles using Adams-Bashforth
      //----------------------------------------------------
      phi += 0.5 * (3.0 * phiDot - phiDot1) * dT;
      if (phi >  PI) phi = -PI;
      if (phi < -PI) phi =  PI;

      tht += 0.5 * (3.0 * thtDot - thtDot1) * dT;
      if (tht >=  HALF_PI) tht =  (HALF_PI - EPSILON);
      if (tht <= -HALF_PI) tht = -(HALF_PI - EPSILON);

      psi += 0.5 * (3.0 * psiDot - psiDot1) * dT;
      if (psi >  PI) psi = -PI;
      if (psi < -PI) psi =  PI;

      //----------------------------------------------------
      // update Euler angles
      //----------------------------------------------------
      pPlr->setEulerAngles(phi, tht, psi);

      //----------------------------------------------------
      // hold current rotational control values for next iteration
      //----------------------------------------------------
      phiDot1 = phiDot;
      thtDot1 = thtDot;
      psiDot1 = psiDot;

      //----------------------------------------------------
      // compute sin, cos of Euler angles
      //----------------------------------------------------
      double sinPhi = std::sin(phi);
      double cosPhi = std::cos(phi);
      double sinTht = std::sin(tht);
      double cosTht = std::cos(tht);
      double sinPsi = std::sin(psi);
      double cosPsi = std::cos(psi);

      //----------------------------------------------------
      // compute local to body axes matrix
      //----------------------------------------------------
      double l1 =  cosTht * cosPsi;
      double l2 =  cosTht * sinPsi;
      double l3 = -sinTht;
      double m1 =  sinPhi * sinTht * cosPsi - cosPhi * sinPsi;
      double m2 =  sinPhi * sinTht * sinPsi + cosPhi * cosPsi;
      double m3 =  sinPhi * cosTht;
      double n1 =  cosPhi * sinTht * cosPsi + sinPhi * sinPsi;
      double n2 =  cosPhi * sinTht * sinPsi - sinPhi * cosPsi;
      double n3 =  cosPhi * cosTht;

      //----------------------------------------------------
      // update p,q,r angular velocity components (body)
      //----------------------------------------------------
      p = phiDot                    +       (-sinTht)*psiDot;
      q =           (cosPhi)*thtDot + (cosTht*sinPhi)*psiDot;
      r =          (-sinPhi)*thtDot + (cosTht*cosPhi)*psiDot;

      //----------------------------------------------------
      // update angular velocities
      //----------------------------------------------------
      pPlr->setAngularVelocities(p, q, r);


      //==============================================================
      // TRANSLATIONAL EOM
      //==============================================================

      //----------------------------------------------------
      // integrate u,v,w velocity components (body)
      //----------------------------------------------------
      u  += 0.5*(3.0*uDot - uDot1)*dT;
      v  += 0.5*(3.0*vDot - vDot1)*dT;
      w  += 0.5*(3.0*wDot - wDot1)*dT;

      //----------------------------------------------------
      // hold current translational control values for next iteration
      //----------------------------------------------------
      uDot1   = uDot;
      vDot1   = vDot;
      wDot1   = wDot;

      //----------------------------------------------------
      // update velocity in NED system
      //----------------------------------------------------
//.........这里部分代码省略.........