C++ Rvector6类代码示例

//------------------------------------------------------------------------------
// void ShowDerivative(const wxString &header, Real *state, Integer satCount)
//------------------------------------------------------------------------------
void  PointMassForce::ShowDerivative(const wxString &header, Real *state,
                                     Integer satCount)
{
   #if DEBUG_PMF_DERV
      static int debugCount2 = 0;
      static bool showDeriv = true;
      
      if (showDeriv)
      {
         MessageInterface::ShowMessage(wxT("%s\n"), header.c_str());
         MessageInterface::ShowMessage(
            wxT(">>>>>=======================================\n"));
         Integer i6;
         Rvector6 stateVec = Rvector6(state);
      
         for (Integer i = 0; i < satCount; i++) 
         {
            i6 = cartIndex + i * 6;
            MessageInterface::ShowMessage
               (wxT("sc#=%d  state=%s\n"), i, stateVec.ToString().c_str());
         
            MessageInterface::ShowMessage
               (wxT("deriv=%f %f %f %f %f %f\n"), deriv[i6], deriv[i6+1], 
                deriv[i6+2], deriv[i6+3], deriv[i6+4], deriv[i6+5]);
         }
         MessageInterface::ShowMessage(
            wxT("=======================================<<<<<\n"));
         
         debugCount2++;
         if (debugCount2 > 10)
            showDeriv = false;
      }
   #endif
}

Rvector6 SpaceObject::GetLastState()
{
   Rvector6 result;
   Real *st = state.GetState();
   result.Set(st[0], st[1], st[2], st[3], st[4], st[5]);
   return result;
}

//---------------------------------------------------------------------------
const Rvector6 Barycenter::GetMJ2000State(const A1Mjd &atTime)
{
   // if it's built-in, get the state from the SpacePoint
   if (isBuiltIn)
   {
      lastStateTime = atTime;
      lastState     = builtInSP->GetMJ2000State(atTime);
      #ifdef DEBUG_BARYCENTER_STATE
         MessageInterface::ShowMessage("Computing state for Barycenter %s, whose builtInSP is %s\n",
               instanceName.c_str(), (builtInSP->GetName()).c_str());
      #endif
      return lastState;
   }
   // otherwise, sum the masses and states
   CheckBodies();
   #ifdef DEBUG_BARYCENTER
      MessageInterface::ShowMessage("Entering BaryCenter::GetMJ2000EqState at time %12.10f\n",
            atTime.Get());
   #endif
   Real     bodyMass = 0.0;
   Rvector3 bodyPos(0.0,0.0,0.0);
   Rvector3 bodyVel(0.0,0.0,0.0);

   Real     weight   = 0.0;
   Rvector3 sumMassPos(0.0,0.0,0.0);
   Rvector3 sumMassVel(0.0,0.0,0.0);
   Rvector6 bodyState;
   Real     sumMass  = GetMass();

   for (unsigned int i = 0; i < bodyList.size() ; i++)
   {
      bodyMass    = ((CelestialBody*) (bodyList.at(i)))->GetMass();
      bodyState   = (bodyList.at(i))->GetMJ2000State(atTime);
      bodyPos     = bodyState.GetR();
      bodyVel     = bodyState.GetV();
      weight      = bodyMass/sumMass;
      #ifdef DEBUG_BARYCENTER
         MessageInterface::ShowMessage("Mass (and weight) of body %s = %12.10f (%12.10f)\n",
               ((bodyList.at(i))->GetName()).c_str(), bodyMass, weight);
         MessageInterface::ShowMessage("    pos = %s\n", (bodyPos.ToString()).c_str());
         MessageInterface::ShowMessage("    vel = %s\n", (bodyVel.ToString()).c_str());
      #endif
      sumMassPos += (weight * bodyPos);
      sumMassVel += (weight * bodyVel);
   }
   #ifdef DEBUG_BARYCENTER
      MessageInterface::ShowMessage("sumMassPos = %s\n",
            (sumMassPos.ToString()).c_str());
   #endif
   lastState.Set(sumMassPos(0), sumMassPos(1), sumMassPos(2),
         sumMassVel(0), sumMassVel(1), sumMassVel(2));
   lastStateTime = atTime;
   return lastState;
}

//------------------------------------------------------------------------------
Real EnvData::GetEnvReal(const std::string &str)
{
   //-------------------------------------------------------
   // 1. Get Spacecraft's central body (It's done in InitializeRefObjects())
   // 2. Get current time (use parameter or sc pointer?)
   // 3. Get spacecraft's position (use parameter?)
   // 4. Call GetDensity() on central body
   //-------------------------------------------------------
   
   if (mSpacecraft == NULL || mSolarSystem == NULL || mOrigin == NULL)
      InitializeRefObjects();

   if (str == "AtmosDensity")
   {
      Real a1mjd = mSpacecraft->GetRealParameter("A1Epoch");
      Rvector6 cartState = mSpacecraft->GetState().GetState();
      
//       // Rvector6 cartState = mSpacecraft->GetStateVector("Cartesian");
//       Rvector6 cartState = mSpacecraft->GetState(0);
//       Real state[6];
//       for (int i=0; i<6; i++)
//          state[i] = cartState[i];
      Real *state = (Real*)cartState.GetDataVector();
      Real density = 0.0;
      
      // Call GetDensity() on if origin is CelestialBody
      if (mOrigin->IsOfType(Gmat::CELESTIAL_BODY))
      {
         if (((CelestialBody*)mOrigin)->GetDensity(state, &density, a1mjd, 1))
         {
            #ifdef DEBUG_ENVDATA_RUN
            MessageInterface::ShowMessage
               ("EnvData::GetEnvReal() mOrigin=%s, a1mjd=%f, state=%s, density=%g\n",
                mOrigin->GetName().c_str(), a1mjd, cartState.ToString().c_str(), density);
            #endif
         }
         else
         {
            #ifdef DEBUG_ENVDATA_RUN
            MessageInterface::ShowMessage
               ("EnvData::GetEnvReal() AtmosphereModel used for %s is NULL\n",
                mOrigin->GetName().c_str());
            #endif
         }
      }
      
      return density;
   }
   else
   {
      throw ParameterException("EnvData::GetEnvReal() Unknown parameter name: " +
                               str);
   }
}

//------------------------------------------------------------------------------
void GeocentricSolarEclipticAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
                                                          bool forceComputation) 
{
   Rvector6 rvSun = secondary->GetMJ2000State(atEpoch) -
                    primary->GetMJ2000State(atEpoch);
   Rvector3 rSun  = rvSun.GetR();
   Rvector3 vSun  = rvSun.GetV();

   Real     rMag  = rSun.GetMagnitude();
   Real     vMag  = vSun.GetMagnitude();

   Rvector3 rUnit = rSun / rMag;
   Rvector3 vUnit = vSun / vMag;
   
   Rvector3 rxv   = Cross(rSun,vSun);
   Real     rxvM  = rxv.GetMagnitude();

   Rvector3 x     = rUnit;
   Rvector3 z     = rxv / rxvM;
   Rvector3 y     = Cross(z,x);

   rotMatrix(0,0) = x(0);
   rotMatrix(0,1) = y(0);
   rotMatrix(0,2) = z(0);
   rotMatrix(1,0) = x(1);
   rotMatrix(1,1) = y(1);
   rotMatrix(1,2) = z(1);
   rotMatrix(2,0) = x(2);
   rotMatrix(2,1) = y(2);
   rotMatrix(2,2) = z(2);

   Rvector3 vR    = vSun / rMag;
   Rvector3 xDot  = vR - x * (x * vR);
   Rvector3 zDot;     // zero vector by default
   Rvector3 yDot  = Cross(z, xDot);

   rotDotMatrix(0,0) = xDot(0);
   rotDotMatrix(0,1) = yDot(0);
   rotDotMatrix(0,2) = zDot(0);
   rotDotMatrix(1,0) = xDot(1);
   rotDotMatrix(1,1) = yDot(1);
   rotDotMatrix(1,2) = zDot(1);
   rotDotMatrix(2,0) = xDot(2);
   rotDotMatrix(2,1) = yDot(2);
   rotDotMatrix(2,2) = zDot(2);

}

int main(int argc, char **argv)
{
   std::cout << "************************************************\n"
             << "*** StateType Conversion Unit Test Program\n"
             << "************************************************\n\n";

   
   StateConverter stateConverter; 

   Rvector6 newState;
   Real state[6];

   state[0] = 7100;
   state[1] = 0.0;
   state[2] = 1300;
   state[3] = 0.0;
   state[4] = 7.35;
   state[5] = 1.0;

   std::cout << "\n--- Beginning with state ----";
   std::cout << std::setprecision(8);
   for (int i=0; i < 6; i++)
       std::cout << "\n[" << i << "]: " << state[i];

  
   std::cout << "\n--- Converting to Keplerian state ----";
   newState = stateConverter.Convert(state,"Cartesian","Keplerian"); 
   for (int i=0; i < 6; i++)
   {
       state[i] = newState.Get(i);
       std::cout << "\n[" << i << "]: " << state[i];
   }

   std::cout << "\n--- Converting to Cartesian state ----";
   newState = stateConverter.Convert(state,"Keplerian","Cartesian"); 
   for (int i=0; i < 6; i++)
   {
       state[i] = newState.Get(i);
       std::cout << "\n[" << i << "]: " << state[i];
   }

   std::cout << "\n********************* End of Testing ********************\n";
}

//------------------------------------------------------------------------------
const Rvector6 BodyFixedPoint::GetMJ2000State(const A1Mjd &atTime)
{
   #ifdef DEBUG_BODYFIXED_STATE
      MessageInterface::ShowMessage("In GetMJ2000State for BodyFixedPoint %s\n",
            instanceName.c_str());
   #endif

   UpdateBodyFixedLocation();
   Real     epoch = atTime.Get();
   Rvector6 bfState;

   // For now I'm ignoring velocity; this assumes bfLocation is kept up-to-date
   bfState.Set(bfLocation[0], bfLocation[1], bfLocation[2], 0.0, 0.0, 0.0);

   // Convert from the body-fixed location to a J2000 location,
   // assuming you have pointer to coordinate systems mj2k and bfcs,
   // where mj2k is a J2000 system and bfcs is BodyFixed
   #ifdef DEBUG_BODYFIXED_STATE
      MessageInterface::ShowMessage("... before call to Convert, epoch = %12.10f\n",
            epoch);
      MessageInterface::ShowMessage(" ... bfcs (%s) = %s  and mj2kcs (%s) = %s\n",
            bfcsName.c_str(), (bfcs? "NOT NULL" : "NULL"),
            mj2kcsName.c_str(), (mj2kcs? "NOT NULL" : "NULL"));
      MessageInterface::ShowMessage("bf state (in bfcs, cartesian) = %s\n",
            (bfState.ToString()).c_str());
      MessageInterface::ShowMessage("SolarSystem is%s NULL\n", (solarSystem? "NOT " : ""));
   #endif
   ccvtr.Convert(epoch, bfState, bfcs, j2000PosVel, mj2kcs);
   #ifdef DEBUG_BODYFIXED_STATE
      MessageInterface::ShowMessage("bf state (in mj2kcs, cartesian) = %s\n",
            (j2000PosVel.ToString()).c_str());
   #endif

   lastStateTime = atTime;
   lastState     = j2000PosVel;
   return j2000PosVel;
}

//------------------------------------------------------------------------------
// void ShowBodyState(const wxString &header, Real time, Rvector6 &rv);
//------------------------------------------------------------------------------
void  PointMassForce::ShowBodyState(const wxString &header, Real time,
                                    Rvector6 &rv)
{
   #if DEBUG_PMF_BODY
      static int debugCount1 = 0;
      static bool showBodyState = true;
      
      if (showBodyState)
      {
         MessageInterface::ShowMessage(wxT("%s\n"), header.c_str());
         MessageInterface::ShowMessage(
            wxT(">>>>>=======================================\n"));
         MessageInterface::ShowMessage(wxT("time=%f  rv=%s\n"), time,
            rv.ToString().c_str());
         MessageInterface::ShowMessage(
            wxT("=======================================<<<<<\n"));
         
         debugCount1++;
         if (debugCount1 > 10)
            showBodyState = false;
      }
   #endif
}

void printState(const std::string &title, const Rvector6 state)
{
    std::cout << "\n--------- " << title << " ---------\n";
    for (int i=0; i < 6; i++)
        std::cout << "[" << i << "]: " << state.Get(i) << std::endl;
}

//------------------------------------------------------------------------------
const Rvector3 SpaceObject::GetMJ2000Velocity(const A1Mjd &atTime)
{
   const Rvector6 rv6 = GetMJ2000State(atTime);
   return (rv6.GetV());
}

//------------------------------------------------------------------------------
// static Rvector6 CartesianToKeplerian(const Rvector6 &cartVec, Real grav,
//                                      Real *ma);
//------------------------------------------------------------------------------
Rvector6 CoordUtil::CartesianToKeplerian(const Rvector6 &cartVec, Real grav, Real *ma)
{
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian called ... \n"));
   #endif

   Real kepl[6];
   Real r[3];
   Real v[3];
   Real tfp;
   Integer  ret;
   Integer  errorCode;

   for (int i=0; i<6; i++)
      kepl[i] = 0.0;
   
   if(grav < 1.0)
   {
      throw UtilityException(wxT("CoordUtil::CartesianToKeplerian() gravity constant ")
                             wxT("too small for conversion to Keplerian elements\n"));
   }
   else
   {
      cartVec.GetR(r);
      cartVec.GetV(v);

      //MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian() r=%f, %f, %f ")
      //                              wxT("v=%f, %f, %f\n"), r[0], r[1], r[2], v[0], v[1], v[2]);
      
      if (CoordUtil::IsRvValid(r,v))
      {
         errorCode = CoordUtil::ComputeCartToKepl(grav, r, v, &tfp, kepl, ma);

         switch (errorCode)
         {
         case 0: // no error
            ret = 1;
            break;
         case 2:
            throw UtilityException
               (wxT("CoordUtil::CartesianToKeplerian() ")
                wxT("Gravity constant too small for conversion to Keplerian elements\n"));
         default:
            throw UtilityException
               (wxT("CoordUtil::CartesianToKeplerian() ")
                wxT("Unable to convert Cartesian elements to Keplerian\n"));
         }
      }
      else
      {
         wxString ss;
         //ss << cartVec;
         throw UtilityException
            (wxT("CoordUtil::CartesianToKeplerian() Invalid Cartesian elements:\n") +
             ss);
      }
   }

   Rvector6 keplVec = Rvector6(kepl[0], kepl[1], kepl[2], kepl[3], kepl[4], kepl[5]);
   return keplVec;

}

//------------------------------------------------------------------------------
void GravityField::Calculate (Real dt, Real state[6], 
                              Real acc[3], Rmatrix33& grad)
{
   #ifdef DEBUG_CALCULATE
      MessageInterface::ShowMessage(
            "Entering Calculate with dt = %12.10f, state = %12.10f  %12.10f  %12.10f  %12.10f  %12.10f  %12.10f\n",
            dt, state[0], state[1], state[2], state[3], state[4], state[5]);
      MessageInterface::ShowMessage("   acc = %12.10f  %12.10f  %12.10f\n", acc[0], acc[1], acc[2]);
   #endif
   Real jday = epoch + GmatTimeConstants::JD_JAN_5_1941 +
               dt/GmatTimeConstants::SECS_PER_DAY;
   // convert to body fixed coordinate system
   Real now = epoch + dt/GmatTimeConstants::SECS_PER_DAY;
   Real tmpState[6];
//   CoordinateConverter cc; - move back to class, for performance
   cc.Convert(now, state, inputCS, tmpState, fixedCS);  // which CSs to use here???
   #ifdef DEBUG_CALCULATE
      MessageInterface::ShowMessage(
            "After Convert, jday = %12.10f, now = %12.10f, and tmpState = %12.10f  %12.10f  %12.10f  %12.10f  %12.10f  %12.10f\n",
            jday, now, tmpState[0], tmpState[1], tmpState[2], tmpState[3], tmpState[4], tmpState[5]);
   #endif
   Rmatrix33 rotMatrix = cc.GetLastRotationMatrix();
#ifdef DEBUG_DERIVATIVES
   MessageInterface::ShowMessage("---->>>> rotMatrix = %s\n", rotMatrix.ToString().c_str());
#endif
   // calculate sun and moon pos
   Real sunpos[3]   = {0.0,0.0,0.0};
   Real moonpos[3]  = {0.0,0.0,0.0};
   Real sunMass     = 0.0;
   Real moonMass    = 0.0;
   // Acceleration
   Real      rotacc[3];
   Rmatrix33 rotgrad;
   bool      useTides;
   // for now, "None" and "SolidAndPole" are the only valid EarthTideModel values
   if ((bodyName == GmatSolarSystemDefaults::EARTH_NAME) && (GmatStringUtil::ToUpper(earthTideModel) == "SOLIDANDPOLE"))
   {
      Real ep = epoch + dt / GmatTimeConstants::SECS_PER_DAY;  // isn't this the same as now?
      CelestialBody* theSun  = solarSystem->GetBody(SolarSystem::SUN_NAME);
      CelestialBody* theMoon = solarSystem->GetBody(SolarSystem::MOON_NAME);
      if (!theSun || !theMoon)
         throw ODEModelException("Solar system does not contain the Sun or Moon for Tide model.");
      Rvector6 sunstateinertial  = theSun->GetState(ep);
      Rvector6 moonstateinertial = theMoon->GetState(ep);
      
      Rvector6 sunstate;
      Rvector6 moonstate;
      cc.Convert(now, sunstateinertial, inputCS, sunstate, fixedCS);
      cc.Convert(now, moonstateinertial, inputCS, moonstate, fixedCS);
      sunstate.GetR(sunpos);
      moonstate.GetR(moonpos);

      sunMass     = theSun->GetMass();
      moonMass    = theMoon->GetMass();
      useTides = true;
   }
   else
      useTides = false;
   #ifdef DEBUG_GRAVITY_EARTH_TIDE
      MessageInterface::ShowMessage("Calling gravityModel->CalculateFullField with useTides = %s\n",
            (useTides? "true" : "false"));
   #endif
   // Get xp and yp from the EOP file
   Real xp, yp, lod;
   Real utcmjd  = TimeConverterUtil::Convert(now, TimeConverterUtil::A1MJD, TimeConverterUtil::UTCMJD,
                 GmatTimeConstants::JD_JAN_5_1941);
   eop->GetPolarMotionAndLod(utcmjd, xp, yp, lod);
   bool computeMatrix = fillAMatrix || fillSTM;

   gravityModel->CalculateFullField(jday, tmpState, degree, order, useTides, sunpos, moonpos, sunMass, moonMass,
                                    xp, yp, computeMatrix, rotacc, rotgrad);
#ifdef DEBUG_DERIVATIVES
   MessageInterface::ShowMessage("after CalculateFullField, rotgrad = %s\n", rotgrad.ToString().c_str());
#endif
   /*
    MessageInterface::ShowMessage
    ("HarmonicField::Calculate pos= %20.14f %20.14f %20.14f\n",
    tmpState[0],tmpState[1],tmpState[2]);
    MessageInterface::ShowMessage
    ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
    rotgrad(0,0),rotgrad(0,1),rotgrad(0,2));
    MessageInterface::ShowMessage
    ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
    rotgrad(1,0),rotgrad(1,1),rotgrad(1,2));
    MessageInterface::ShowMessage
    ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n",
    rotgrad(2,0),rotgrad(2,1),rotgrad(2,2));
    */
   
   // Convert back to target CS
   InverseRotate (rotMatrix,rotacc,acc);
   grad = rotMatrix.Transpose() * rotgrad * rotMatrix;
#ifdef DEBUG_DERIVATIVES
   MessageInterface::ShowMessage("at end of Calculate, after rotation, grad = %s\n", grad.ToString().c_str());
#endif
}

//---------------------------------------------------------------------------
const Rvector3 Barycenter::GetMJ2000Position(const A1Mjd &atTime)
{
   Rvector6 tmp = GetMJ2000State(atTime);
   return (tmp.GetR());
}

//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
                                                   bool forceComputation)
{
   if (!primary)
      throw CoordinateSystemException("Primary \"" + primaryName +
         "\" is not yet set in object referenced coordinate system!");

   if (!secondary)
      throw CoordinateSystemException("Secondary \"" + secondaryName +
         "\" is not yet set in object referenced coordinate system!");

   
   if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
   {
      CoordinateSystemException cse;
      cse.SetDetails("For object referenced axes, axes are improperly "
                     "defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
                     xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
      throw cse;
   }
   
   if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
   {
      CoordinateSystemException cse;
      cse.SetDetails("For object referenced axes, too many axes are defined.\n"
                     "XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
                     xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
      throw cse;
   }
   
   SpacePoint *useAsSecondary = secondary;
//   if (!useAsSecondary)  useAsSecondary = origin;
   Rvector6 rv     = useAsSecondary->GetMJ2000State(atEpoch) -
                     primary->GetMJ2000State(atEpoch);
   #ifdef DEBUG_ROT_MATRIX
      if (visitCount == 0)
      {
         MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
               primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
         visitCount++;
      }
   #endif

   #ifdef DEBUG_ROT_MATRIX
      if (visitCount == 0)
      {
         std::stringstream ss;
         ss.precision(30);
         ss << " ----------------- rv Earth to Moon (truncated)    = "
              << rv << std::endl;

         MessageInterface::ShowMessage("%s\n", ss.str().c_str());
         visitCount++;
      }
   #endif

   Rvector3 a     =  useAsSecondary->GetMJ2000Acceleration(atEpoch) -
                     primary->GetMJ2000Acceleration(atEpoch);
   
   Rvector3 r      = rv.GetR();
   Rvector3 v      = rv.GetV();
   Rvector3 n     =  Cross(r,v);
   Rvector3 rUnit = r.GetUnitVector();
   Rvector3 vUnit = v.GetUnitVector();
   Rvector3 nUnit = n.GetUnitVector();
   Real     rMag  = r.GetMagnitude();
   Real     vMag  = v.GetMagnitude();
   Real     nMag  = n.GetMagnitude();
   // check for divide-by-zero
   if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
   {
      std::string errmsg = "Object referenced axis system named \"";
      errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
      throw CoordinateSystemException(errmsg);
   }

   Rvector3 rDot  = (v / rMag) - (rUnit / rMag) * (rUnit * v);
   Rvector3 vDot  = (a / vMag) - (vUnit / vMag) * (vUnit * a);
   Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
   Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
   bool     xUsed = true, yUsed = true, zUsed = true;


   // determine the x-axis
   if ((xAxis == "R") || (xAxis == "r"))
   {
      xUnit = rUnit;
      xDot  = rDot;
   }
   else if ((xAxis == "-R") || (xAxis == "-r"))
   {
      xUnit = -rUnit;
      xDot  = -rDot;
   }
   else if ((xAxis == "V") || (xAxis == "v"))
   {
      xUnit = vUnit;
      xDot  = vDot;
   }
//.........这里部分代码省略.........

void NadirPointing::ComputeCosineMatrixAndAngularVelocity(Real atTime)
{
   //#ifdef DEBUG_NADIRPOINTING
   //#endif

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("In NadirPointing, computing at time %le\n",
            atTime);
      MessageInterface::ShowMessage("ModeOfConstraint = %s\n", modeOfConstraint.c_str());
      if (!owningSC) MessageInterface::ShowMessage("--- owningSC is NULL!!!!\n");
      if (!refBody) MessageInterface::ShowMessage("--- refBody is NULL!!!!\n");
   #endif

   if (!isInitialized || needsReinit)
   {
      Initialize();
      // Check for unset reference body pointer
      if (!refBody)
      {
         std::string attEx  = "Reference body ";
         attEx             += refBodyName + " not defined for attitude of type \"";
         attEx             += "NadirPointing\"";
         throw AttitudeException(attEx);
      }
   }

   /// using a spacecraft object, *owningSC
   A1Mjd theTime(atTime);

   Rvector6 scState = ((SpaceObject*) owningSC)->GetMJ2000State(theTime);
   Rvector6 refState = refBody->GetMJ2000State(theTime);
    
   Rvector3 pos(scState[0] - refState[0], scState[1] - refState[1],  scState[2] - refState[2]);
   Rvector3 vel(scState[3] - refState[3], scState[4] - refState[4],  scState[5] - refState[5]);

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("   scState  = %s\n", scState.ToString().c_str());
      MessageInterface::ShowMessage("   refState = %s\n", refState.ToString().c_str());
      MessageInterface::ShowMessage("   pos      = %s\n", pos.ToString().c_str());
      MessageInterface::ShowMessage("   vel      = %s\n", vel.ToString().c_str());
   #endif

   // Error message
   if ( bodyAlignmentVector.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("The size of BodyAlignmentVector is too small.");
   }
   else if ( bodyConstraintVector.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("The size of BodyConstraintVector is too small.");
   }
   else if ( bodyAlignmentVector.GetUnitVector()*bodyConstraintVector.GetUnitVector() > ( 1.0 - 1.0e-5) )
   {
	   throw AttitudeException("BodyAlignmentVector and BodyConstraintVector are the same.");
   }
   else if ( pos.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("A position vector is zero.");
   }
   else if ( vel.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("A velocity vector is zero.");
   }
   else if ( Cross(pos,vel).GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("Cross product of position and velocity is zero: LVLH frame cannot be defined.");
   }
   
   Rvector3 normal = Cross(pos,vel);

   Rvector3 xhat = pos/pos.GetMagnitude();
   Rvector3 yhat = normal/normal.GetMagnitude();
   Rvector3 zhat = Cross(xhat,yhat);

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("   normal  = %s\n", normal.ToString().c_str());
      MessageInterface::ShowMessage("   xhat    = %s\n", xhat.ToString().c_str());
      MessageInterface::ShowMessage("   yhat    = %s\n", yhat.ToString().c_str());
      MessageInterface::ShowMessage("   zhat    = %s\n", zhat.ToString().c_str());
   #endif

   // RiI calculation (from inertial to LVLH)
   Rmatrix33 RIi;
   RIi(0,0) = xhat(0);
   RIi(1,0) = xhat(1);
   RIi(2,0) = xhat(2);
   RIi(0,1) = yhat(0);
   RIi(1,1) = yhat(1);
   RIi(2,1) = yhat(2);
   RIi(0,2) = zhat(0);
   RIi(1,2) = zhat(1);
   RIi(2,2) = zhat(2);
   
   // Set alignment/constraint vector in body frame
   if ( modeOfConstraint == "OrbitNormal" )
   {
	   referenceVector[0] = -1;
	   referenceVector[1] = 0;
	   referenceVector[2] = 0;

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