//------------------------------------------------------------------------------
// Returns the attitude (DCM) state from inertial-to-body at the specified
// input time.
//------------------------------------------------------------------------------
Rmatrix33 CCSDSAEMEulerAngleSegment::GetState(Real atEpoch)
{
   #ifdef DEBUG_AEM_EULER_GET_STATE
      MessageInterface::ShowMessage("====== Entering EulerSegment::GetState with epoch = %lf, dataStore size = %d\n",
            atEpoch, (Integer) dataStore.size());
   #endif
   // DetermineState will look for an exact match; if so,
   // return the state at that time; if not, then return the last
   // state (if interpolation degree = 0) or else, interpolate
   // to the requested time
   Rvector  eulerAngles(3);
   eulerAngles = DetermineState(atEpoch);
   #ifdef DEBUG_AEM_EULER_GET_STATE
      MessageInterface::ShowMessage("          eulerAngles (deg) from DetermineState are: %12.10f  %12.10f  %12.10f\n",
            eulerAngles[0] * GmatMathConstants::DEG_PER_RAD,
            eulerAngles[1] * GmatMathConstants::DEG_PER_RAD,
            eulerAngles[2] * GmatMathConstants::DEG_PER_RAD);
      MessageInterface::ShowMessage("          and euler sequence is: %d   %d   %d\n", euler1, euler2, euler3);
   #endif

   // Conversion method has to have an Rvector3
   Rvector3  theEulerAngles(eulerAngles(0), eulerAngles(1), eulerAngles(2));
   Rmatrix33 theDCM = AttitudeConversionUtility::ToCosineMatrix(
                      theEulerAngles, euler1, euler2, euler3);
   #ifdef DEBUG_AEM_EULER_GET_STATE
      MessageInterface::ShowMessage("About to Exit EulerSegment::GetState, DCM = %s\n",
            theDCM.ToString().c_str());
   #endif
   if (inertialToBody) return theDCM;
   else                return theDCM.Transpose();
}
Exemple #2
0
//------------------------------------------------------------------------------
Rmatrix33 ITRFAxes::Skew(Rvector3 vec)
{
	Rmatrix33 r;
	r.SetElement(0,0,0.0);					r.SetElement(0,1,-vec.GetElement(2));	r.SetElement(0,2,vec.GetElement(1));
	r.SetElement(1,0,vec.GetElement(2));	r.SetElement(1,1,0.0);					r.SetElement(1,2,-vec.GetElement(0));
	r.SetElement(2,0,-vec.GetElement(1));	r.SetElement(2,1,vec.GetElement(0));	r.SetElement(2,2,0.0);

	return r;
}
Exemple #3
0
//------------------------------------------------------------------------------
Rmatrix33 ITRFAxes::R3(Real angle)
{
	Rmatrix33 r;
	Real c = cos(angle);
	Real s = sin(angle);
	r.SetElement(0,0,c);	r.SetElement(0,1,s);	r.SetElement(0,2,0.0);
	r.SetElement(1,0,-s);	r.SetElement(1,1,c);	r.SetElement(1,2,0.0);
	r.SetElement(2,0,0.0);	r.SetElement(2,1,0.0);	r.SetElement(2,2,1.0);

	return r;
}
void CoordinateConverter::RotationMatrixFromICRFToFK5(const A1Mjd &atEpoch)
{
   Real theEpoch = atEpoch.Get();
   #ifdef DEBUG_ICRF_TOFK5
         MessageInterface::ShowMessage(
            "Enter CoordinateConverter::RotationMatrixFromICRFToFK5 at epoch %18.12lf; \n\n", theEpoch);
   #endif

   // Specify Euler rotation vector for theEpoch:
   Real vec[3];
   ICRFFile* icrfFile = ICRFFile::Instance();
   icrfFile->Initialize();
   icrfFile->GetICRFRotationVector(theEpoch, &vec[0], 3, 9);

   // Calculate rotation matrix based on Euler rotation vector:
   Real angle = GmatMathUtil::Sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);
   Real a[3];
   a[0] = vec[0]/angle; a[1] = vec[1]/angle; a[2] = vec[2]/angle;
   Real c = GmatMathUtil::Cos(angle);
   Real s = GmatMathUtil::Sin(angle);

   // rotation matrix from FK5 to ICRF:
   Rmatrix33 rotM;
   rotM.SetElement(0,0, c+a[0]*a[0]*(1-c));
   rotM.SetElement(0,1, a[0]*a[1]*(1-c)+a[2]*s);
   rotM.SetElement(0,2, a[0]*a[2]*(1-c)-a[1]*s);
   rotM.SetElement(1,0, a[0]*a[1]*(1-c)-a[2]*s);
   rotM.SetElement(1,1, c+a[1]*a[1]*(1-c));
   rotM.SetElement(1,2, a[1]*a[2]*(1-c)+a[0]*s);
   rotM.SetElement(2,0, a[0]*a[2]*(1-c)+a[1]*s);
   rotM.SetElement(2,1, a[1]*a[2]*(1-c)-a[0]*s);
   rotM.SetElement(2,2, c+a[2]*a[2]*(1-c));

   // rotation matrix from ICRF to FK5:
   icrfToFK5 = rotM.Transpose();

   // rotation dot matrix from ICRF to FK5:
   icrfToFK5Dot.Set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);

   #ifdef DEBUG_ICRF_TOFK5
      MessageInterface::ShowMessage("theEpoch  = %18.12lf\n",theEpoch);

      MessageInterface::ShowMessage("rotation vector = %18.12e %18.12e %18.12e\n", vec[0], vec[1], vec[2]);
      MessageInterface::ShowMessage("R(0,0)=%18.12e,  R(0,1)=%18.12e,  R(0,2)=%18.12e\n",icrfToFK5(0,0),icrfToFK5(0,1),icrfToFK5(0,2));
      MessageInterface::ShowMessage("R(1,0)=%18.12e,  R(1,1)=%18.12e,  R(1,2)=%18.12e\n",icrfToFK5(1,0),icrfToFK5(1,1),icrfToFK5(1,2));
      MessageInterface::ShowMessage("R(2,0)=%18.12e,  R(2,1)=%18.12e,  R(2,2)=%18.12e\n",icrfToFK5(2,0),icrfToFK5(2,1),icrfToFK5(2,2));

      MessageInterface::ShowMessage("Rdot(0,0)=%18.12e,  Rdot(0,1)=%18.12e,  Rdot(0,2)=%18.12e\n",icrfToFK5Dot(0,0),icrfToFK5Dot(0,1),icrfToFK5Dot(0,2));
      MessageInterface::ShowMessage("Rdot(1,0)=%18.12e,  Rdot(1,1)=%18.12e,  Rdot(1,2)=%18.12e\n",icrfToFK5Dot(1,0),icrfToFK5Dot(1,1),icrfToFK5Dot(1,2));
      MessageInterface::ShowMessage("Rdot(2,0)=%18.12e,  Rdot(2,1)=%18.12e,  Rdot(2,2)=%18.12e\n\n\n",icrfToFK5Dot(2,0),icrfToFK5Dot(2,1),icrfToFK5Dot(2,2));
   #endif

   #ifdef DEBUG_ICRF_TOFK5
      MessageInterface::ShowMessage("NOW exiting CoordinateConverter::RotationMatrixFromICRFToFK5 ...\n\n");
   #endif

}
Exemple #5
0
//------------------------------------------------------------------------------
//  Rvector3 operator/(const Rmatrix33& m) const
//------------------------------------------------------------------------------
Rvector3 Rvector3::operator/(const Rmatrix33& m) const
{
    Rmatrix33 invM;
    try
    {
        invM = m.Inverse();
    }
    catch (Rmatrix::IsSingular)
    {
        throw;
    }
    return (*this) * invM;
}
Exemple #6
0
// ********** JPD added these ***********
//--------------------------------------------------------------------
void GravityField::InverseRotate (Rmatrix33& rot, const Real in[3], Real out[3])
{
   const Real *rm = rot.GetDataVector();
   const Real  rmt[9] = {rm[0], rm[3], rm[6],
      rm[1], rm[4], rm[7],
      rm[2], rm[5], rm[8]};
   for (Integer p = 0; p < 3; ++p)
   {
      Integer p3 = 3*p;
      out[p] = rmt[p3]   * in[0] +
      rmt[p3+1] * in[1] +
      rmt[p3+2] * in[2];
   }
   
}
Exemple #7
0
//------------------------------------------------------------------------------
// void ConvertDeltaVToInertial(Real *dv, Real *dvInertial, Real epoch)
//------------------------------------------------------------------------------
void Burn::ConvertDeltaVToInertial(Real *dv, Real *dvInertial, Real epoch)
{
   #ifdef DEBUG_BURN_CONVERT
   MessageInterface::ShowMessage
      ("Burn::ConvertDeltaVToInertial(), usingLocalCoordSys=%d, coordSystemName='%s', "
       "coordSystem=<%p>'%s'\n", usingLocalCoordSys, coordSystemName.c_str(),
       coordSystem, coordSystem ? coordSystem->GetName().c_str() : "NULL");
   #endif
   
   if (usingLocalCoordSys && localCoordSystem == NULL)
   {      
      throw BurnException
         ("Unable to convert burn elements to Inertial, the local Coordinate "
          "System has not been created");
   }
   else if (!usingLocalCoordSys && coordSystem == NULL)
   {
      throw BurnException
         ("Unable to convert burn elements to Inertial, the Coordinate "
          "System has not been set");      
   }
   
   Real inDeltaV[6], outDeltaV[6];
   for (Integer i=0; i<3; i++)
      inDeltaV[i] = dv[i];
   for (Integer i=3; i<6; i++)
      inDeltaV[i] = 0.0;
   
   // if not using local CS, use ref CoordinateSystem
   if (!usingLocalCoordSys)
   {     
      // Now rotate to MJ2000Eq axes, we don't want to translate so
      // set coincident to true
      coordSystem->ToBaseSystem(epoch, inDeltaV, outDeltaV, true);  // @todo - need ToMJ2000Eq here?
      
      #ifdef DEBUG_BURN_CONVERT_ROTMAT
      Rmatrix33 rotMat = coordSystem->GetLastRotationMatrix();
      MessageInterface::ShowMessage
         ("rotMat=\n%s\n", rotMat.ToString(16, 20).c_str());
      #endif
      
      dvInertial[0] = outDeltaV[0];
      dvInertial[1] = outDeltaV[1];
      dvInertial[2] = outDeltaV[2];
   }
   else
   {
      // if MJ2000Eq axes rotation matrix is always identity matrix
      if (isMJ2000EqAxes)
      {
         dvInertial[0] = dv[0];
         dvInertial[1] = dv[1];
         dvInertial[2] = dv[2];
      }
      else if (isSpacecraftBodyAxes)
      {
         Rvector3 inDeltaV(dv[0], dv[1], dv[2]);
         Rvector3 outDeltaV;
         // Get attitude matrix from Spacecraft and transpose since
         // attitude matrix from spacecraft gives rotation matrix from
         // inertial to body
         Rmatrix33 inertialToBody = spacecraft->GetAttitude(epoch);
         Rmatrix33 rotMat = inertialToBody.Transpose();
         #ifdef DEBUG_BURN_CONVERT_ROTMAT
         MessageInterface::ShowMessage
            ("for local Spacecraft body ----- rotMat=\n%s\n", rotMat.ToString(16, 20).c_str());
         #endif
         outDeltaV = inDeltaV * rotMat;
         for (Integer i=0; i<3; i++)
            dvInertial[i] = outDeltaV[i];
      }
      else
      {         
//         // Now rotate to MJ2000Eq axes
//         localCoordSystem->ToMJ2000Eq(epoch, inDeltaV, outDeltaV, true);
         // Now rotate to base system axes
         localCoordSystem->ToBaseSystem(epoch, inDeltaV, outDeltaV, true);   // @todo - need ToMJ2000Eq here?

         dvInertial[0] = outDeltaV[0];
         dvInertial[1] = outDeltaV[1];
         dvInertial[2] = outDeltaV[2];
      }
   }
   
   #ifdef DEBUG_BURN_CONVERT
   MessageInterface::ShowMessage
      ("Burn::ConvertDeltaVToInertial() returning\n"
       "           dv = %f %f %f\n   dvInertial = %f %f %f\n",
       dv[0], dv[1], dv[2], dvInertial[0], dvInertial[1], dvInertial[2]);
   #endif
}
Exemple #8
0
//------------------------------------------------------------------------------
bool GravityField::GetDerivatives(Real * state, Real dt, Integer dvorder,
      const Integer id)
{
   #ifdef DEBUG_FIRST_CALL
      if (firstCallFired == false)
      {
         MessageInterface::ShowMessage(
            "GravityField(%s) inputs:\n"
            "   state = [%.10lf %.10lf %.10lf %.16lf %.16lf %.16lf]\n"
            "   dt = %.10lf\n   dvorder = %d\n",
            instanceName.c_str(), state[0], state[1], state[2], state[3],
            state[4], state[5], dt, dvorder);
      }
   #endif

   // We may want to do this down the road:
//   if (fabs(state[0]) + fabs(state[1]) + fabs(state[2]) < minimumDistance)
//      throw ODEModelException("A harmonic gravity field is being computed "
//            "inside of the " + bodyName + ", which is not allowed");

   if ((dvorder > 2) || (dvorder < 1))
      return false;

   #ifdef DEBUG_GRAVITY_FIELD
      MessageInterface::ShowMessage("%s %d %s %le %s  %le %le %le %le %le %le\n",
          "Entered GravityField::GetDerivatives with order", dvorder, "dt = ",
          dt, "and state\n",
          state[0], state[1], state[2], state[3], state[4], state[5]);
      MessageInterface::ShowMessage("cartesianCount = %d, stmCount = %d, aMatrixCount = %d\n",
            cartesianCount, stmCount, aMatrixCount);
      MessageInterface::ShowMessage("fillCartesian = %s, fillSTM = %s, fillAMatrix = %s\n",
            (fillCartesian? "true" : "false"), (fillSTM? "true" : "false"), (fillAMatrix? "true" : "false"));
      MessageInterface::ShowMessage("cartesianStart = %d, stmStart = %d, aMatrixStart = %d\n",
            cartesianStart, stmStart, aMatrixStart);
   #endif

/// @todo Optimize this code -- May be possible to make GravityField calculations more efficient


   if ((cartesianCount < 1)  && (stmCount < 1) && (aMatrixCount < 1))
      throw ODEModelException(
         "GravityField requires at least one spacecraft.");

   // todo: Move into header; this flag is used to decide if the velocity terms
   // are copied into the position derivatives for first order integrators, so
   // when the GravityField is set to work at non-central bodies, the detection
   // will need to happen in initialization.
   Real satState[6];
   Integer nOffset;

   now = epoch + dt/GmatTimeConstants::SECS_PER_DAY;

   #ifdef DEBUG_GRAV_COORD_SYSTEM
       MessageInterface::ShowMessage(
         "------ body = %s\n------ inputCS = %s\n------ targetCS = %s"
         "\n------ fixedCS = %s\n",
         body->GetName().c_str(),     (inputCS == NULL? "NULL" : inputCS->GetName().c_str()),
         (targetCS == NULL? "NULL" : targetCS->GetName().c_str()), (fixedCS == NULL? "NULL" : fixedCS->GetName().c_str()));
   #endif



   #ifdef DEBUG_FIRST_CALL
      if (firstCallFired == false)
      {
    	 CelestialBody *targetBody = (CelestialBody*) targetCS->GetOrigin();
    	 CelestialBody *fixedBody  = (CelestialBody*) fixedCS->GetOrigin();
         MessageInterface::ShowMessage(
            "   Epoch = %.12lf\n   targetBody = %s\n   fixedBody = %s\n",
            now.Get(), targetBody->GetName().c_str(),
            fixedBody->GetName().c_str());
         MessageInterface::ShowMessage(
            "------ body = %s\n------ inputCS = %s\n------ targetCS = %s\n"
            "------ fixedCS = %s\n",
            body->GetName().c_str(),     inputCS->GetName().c_str(),
            targetCS->GetName().c_str(), fixedCS->GetName().c_str());
      }
   #endif


   if (fillCartesian || fillAMatrix || fillSTM)
   {
      // See assumption 1, above
      if ((cartesianCount < stmCount) || (cartesianCount < aMatrixCount))
      {
         throw ODEModelException("GetDerivatives: cartesianCount < stmCount or aMatrixCount\n");
      }
      Real originacc[3] = { 0.0,0.0,0.0 };  // JPD code
      Rmatrix33 origingrad (0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
      Rmatrix33 emptyGradient(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
      Rmatrix33 gradnew (0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
      if (body != forceOrigin)
      {
         Real originstate[6] = { 0.0,0.0,0.0,0.0,0.0,0.0 };
         Calculate(dt,originstate,originacc,origingrad);
#ifdef DEBUG_DERIVATIVES
      MessageInterface::ShowMessage("---------> origingrad = %s\n", origingrad.ToString().c_str());
#endif
      }

      for (Integer n = 0; n < cartesianCount; ++n)
      {
         nOffset = cartesianStart + n * stateSize;
         for (Integer i = 0; i < 6; ++i)
            satState[i] = state[i+nOffset];

         Real accnew[3];  // JPD code
         gradnew = emptyGradient;
         Calculate(dt,satState,accnew,gradnew);
         if (body != forceOrigin)
         {
            for (Integer i=0;  i<=2;  ++i)
               accnew[i] -= originacc[i];
            gradnew -= origingrad;
#ifdef DEBUG_DERIVATIVES
      MessageInterface::ShowMessage("---------> body not equal to forceOrigin\n");
#endif
         }
#ifdef DEBUG_DERIVATIVES
      MessageInterface::ShowMessage("---------> gradnew (%d) = %s\n", n, gradnew.ToString().c_str());
#endif
         
         // Fill Derivatives
         switch (dvorder)
         {
            case 1:
               deriv[0+nOffset] = satState[3];
               deriv[1+nOffset] = satState[4];
               deriv[2+nOffset] = satState[5];
               deriv[3+nOffset] = accnew[0];
               deriv[4+nOffset] = accnew[1];
               deriv[5+nOffset] = accnew[2];
               break;

            case 2:
               deriv[0+nOffset] = accnew[0];
               deriv[1+nOffset] = accnew[1];
               deriv[2+nOffset] = accnew[2];
               deriv[3+nOffset] = 0.0;
               deriv[4+nOffset] = 0.0;
               deriv[5+nOffset] = 0.0;
               break;
         }

#ifdef DEBUG_DERIVATIVES
         for (Integer ii = 0 + nOffset; ii < 6+nOffset; ii++)
                     MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", ii, deriv[ii]);
#endif
         if (fillSTM)
         {
            Real aTilde[36];
            Integer element;
            // @todo Add the use of the GetAssociateIndex() method here to get index into state array
            //       (See assumption 1, above)
            if (n <= stmCount)
            {
               Integer i6 = stmStart + n * 36;

               // Calculate A-tilde
               aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
               aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
               aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
               aTilde[ 9] = aTilde[10] = aTilde[11] =
               aTilde[12] = aTilde[13] = aTilde[14] =
               aTilde[15] = aTilde[16] = aTilde[17] =
               aTilde[21] = aTilde[22] = aTilde[23] =
               aTilde[27] = aTilde[28] = aTilde[29] =
               aTilde[33] = aTilde[34] = aTilde[35] = 0.0;

               // fill in the lower left quadrant with the calculated gradient values
               aTilde[18] = gradnew(0,0);
               aTilde[19] = gradnew(0,1);
               aTilde[20] = gradnew(0,2);
               aTilde[24] = gradnew(1,0);
               aTilde[25] = gradnew(1,1);
               aTilde[26] = gradnew(1,2);
               aTilde[30] = gradnew(2,0);
               aTilde[31] = gradnew(2,1);
               aTilde[32] = gradnew(2,2);

               for (Integer j = 0; j < 6; j++)
               {
                  for (Integer k = 0; k < 6; k++)
                  {
                     element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
                     MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", (i6+element), aTilde[element]);
#endif
                     deriv[i6+element] = aTilde[element];
                  }
               }
            }
         }

         if (fillAMatrix)
         {
            Real aTilde[36];
            Integer element;
            // @todo Add the use of the GetAssociateIndex() method here to get index into state array
            //       (See assumption 1, above)
            if (n <= aMatrixCount)
            {
               Integer i6 = aMatrixStart + n * 36;

               // Calculate A-tilde
               aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
               aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
               aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
               aTilde[ 9] = aTilde[10] = aTilde[11] =
               aTilde[12] = aTilde[13] = aTilde[14] =
               aTilde[15] = aTilde[16] = aTilde[17] =
               aTilde[21] = aTilde[22] = aTilde[23] =
               aTilde[27] = aTilde[28] = aTilde[29] =
               aTilde[33] = aTilde[34] = aTilde[35] = 0.0;

               // fill in the lower left quadrant with the calculated gradient values
               aTilde[18] = gradnew(0,0);
               aTilde[19] = gradnew(0,1);
               aTilde[20] = gradnew(0,2);
               aTilde[24] = gradnew(1,0);
               aTilde[25] = gradnew(1,1);
               aTilde[26] = gradnew(1,2);
               aTilde[30] = gradnew(2,0);
               aTilde[31] = gradnew(2,1);
               aTilde[32] = gradnew(2,2);

               for (Integer j = 0; j < 6; j++)
               {
                  for (Integer k = 0; k < 6; k++)
                  {
                     element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
                     MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", (i6+element), aTilde[element]);
#endif
                     deriv[i6+element] = aTilde[element];
                  }
               }
            }
         }

      }  // end for
   }

   #ifdef DEBUG_FIRST_CALL
      if (firstCallFired == false)
      {
         if (body->GetName() == "Mars")
         {
         MessageInterface::ShowMessage(
            "   GravityField[%s <> %s] --> mu = %lf, origin = %s, [%.10lf %.10lf "
            "%.10lf %.16lf %.16lf %.16lf]\n",
            instanceName.c_str(), body->GetName().c_str(), mu,
            targetCS->GetOriginName().c_str(),
            deriv[0], deriv[1], deriv[2], deriv[3], deriv[4], deriv[5]);
         firstCallFired = true;
         }
      }
   #endif

   return true;
}
Exemple #9
0
//------------------------------------------------------------------------------
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
}
Exemple #10
0
//---------------------------------------------------------------------------
void SpiceAttitudeKernelReader::GetTargetOrientation(const wxString &objectName,
                                                     Integer           naifID,
                                                     Integer           forFrameNaifId,
                                                     const A1Mjd       &atTime,
//                                                     Real              tolerance,
                                                     Rmatrix33         &r33,
                                                     Rvector3          &angVel,
                                                     const wxString &referenceFrame)
{
   #ifdef DEBUG_CK_READING
      MessageInterface::ShowMessage(wxT("Entering GetTargetOrientation for object %s, with NAIF ID %d, at time %12.10f, with frame = %s\n"),
         objectName.c_str(), naifID, atTime.Get(), referenceFrame.c_str());
   #endif
   wxString objectNameToUse = objectName;

   objectNameToUse       = GmatStringUtil::ToUpper(objectNameToUse);
   objectNameSPICE       = objectNameToUse.char_str();
   naifIDSPICE           = naifID;
   frameNaifIDSPICE      = forFrameNaifId;
   referenceFrameSPICE   = referenceFrame.char_str();
   etSPICE               = A1ToSpiceTime(atTime.Get());

//   boddef_c(objectNameSPICE, naifIDSPICE);        // CSPICE method to set NAIF ID for an object - is this valid for spacecraft?
   // Convert the time (in TDB) to spacecaft ticks
   SpiceDouble scTime;
   sce2c_c(naifIDSPICE, etSPICE, &scTime);
   if (failed_c())
   {
      ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
      SpiceInt       numChar  = MAX_LONG_MESSAGE_VALUE;
      //SpiceChar      err[MAX_LONG_MESSAGE_VALUE];
      SpiceChar      *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
      getmsg_c(option, numChar, err);
      wxString errStr(wxString::FromAscii(err));
      wxString errmsg = wxT("Error getting spacecraft time (ticks) for object \"");
      errmsg += objectName + wxT("\".  Message received from CSPICE is: ");
      errmsg += errStr + wxT("\n");
      reset_c();
      delete [] err;
      throw UtilityException(errmsg);
   }
   // get the tolerance in spacecraft clock ticks
   wxString    tolerance = wxT("01");  // this should probably be user input, or set as a constant
   ConstSpiceChar *tol = tolerance.char_str();
   SpiceDouble    tolTicks;
   sctiks_c(naifIDSPICE, tol, &tolTicks);
   if (failed_c())
   {
      ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
      SpiceInt       numChar  = MAX_LONG_MESSAGE_VALUE;
      //SpiceChar      err[MAX_LONG_MESSAGE_VALUE];
      SpiceChar      *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
      getmsg_c(option, numChar, err);
      wxString errStr(wxString::FromAscii(err));
      wxString errmsg = wxT("Error getting tolerance (ticks) for object \"");
      errmsg += objectName + wxT("\".  Message received from CSPICE is: ");
      errmsg += errStr + wxT("\n");
      reset_c();
      delete [] err;
      throw UtilityException(errmsg);
   }
   #ifdef DEBUG_CK_READING
      MessageInterface::ShowMessage(wxT("First, check for coverage for object \"%s\", with NAIF ID %d\n"),
         objectName.c_str(), naifID);
   #endif
   Real beginCov = 0.0;
   Real endCov   = 0.0;
   GetCoverageStartAndEnd(loadedKernels, forFrameNaifId, beginCov, endCov, false);

   // Now get the C-matrix and angular velocity at the requested time
   SpiceDouble    cmat[3][3];
   SpiceDouble    av[3];
   SpiceBoolean   found;
   SpiceDouble    clkout;
   #ifdef DEBUG_CK_READING
      MessageInterface::ShowMessage(wxT("about to call ckgpav: \n"));
      MessageInterface::ShowMessage(wxT("   NAIF ID  = %d\n")
                                    wxT("   etSPICE  = %12.10f\n")
                                    wxT("   scTime   = %12.10fn")
                                    wxT("   tolTicks = %12.10f\n")
                                    wxT("   refFrame = %s\n"),
         (Integer) naifIDSPICE, (Real) etSPICE, (Real) scTime, (Real) tolTicks,
         referenceFrame.c_str());
   #endif
   ckgpav_c(frameNaifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
//   ckgpav_c(naifIDSPICE, scTime, tolTicks, referenceFrameSPICE, cmat, av, &clkout, &found);
   if (failed_c())
   {
      ConstSpiceChar option[] = "LONG"; // retrieve long error message, for now
      SpiceInt       numChar  = MAX_LONG_MESSAGE_VALUE;
      //SpiceChar      err[MAX_LONG_MESSAGE_VALUE];
      SpiceChar      *err = new SpiceChar[MAX_LONG_MESSAGE_VALUE];
      getmsg_c(option, numChar, err);
      wxString errStr(wxString::FromAscii(err));
      wxString errmsg = wxT("Error getting C-matrix and/or angular velocity for object \"");
      errmsg += objectName + wxT("\".  Message received from CSPICE is: ");
      errmsg += errStr + wxT("\n");
      reset_c();
      delete [] err;
      throw UtilityException(errmsg);
   }
   if (found == SPICEFALSE)
   {
      wxString errmsg = wxT("Pointing data for object ");
      errmsg += objectName + wxT(" not found on loaded CK/SCLK kernels.\n");
      throw UtilityException(errmsg);
   }
   #ifdef DEBUG_CK_READING
      MessageInterface::ShowMessage(wxT("results from ckgpav: \n"));
      MessageInterface::ShowMessage(wxT("   cosMat = %12.10f  %12.10f  %12.10f\n")
                                    wxT("            %12.10f  %12.10f  %12.10f\n")
                                    wxT("            %12.10f  %12.10f  %12.10f\n"),
                                    (Real)cmat[0][0], (Real)cmat[0][1], (Real)cmat[0][2],
                                    (Real)cmat[1][0], (Real)cmat[1][1], (Real)cmat[1][2],
                                    (Real)cmat[2][0], (Real)cmat[2][1], (Real)cmat[2][2]);
      MessageInterface::ShowMessage(wxT("   angvel = %12.10f  %12.10f  %12.10f\n"),
                                   (Real)av[0], (Real)av[1], (Real)av[2]);
      MessageInterface::ShowMessage(wxT("   and clkout = %12.10f\n"), (Real) clkout);
   #endif
   // Set output values
   r33.Set(cmat[0][0], cmat[0][1], cmat[0][2],
           cmat[1][0], cmat[1][1], cmat[1][2],
           cmat[2][0], cmat[2][1], cmat[2][2]);
   angVel.Set(av[0], av[1], av[2]);

}
Exemple #11
0
//------------------------------------------------------------------------------
void ITRFAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
                                            bool forceComputation) 
{
   #ifdef DEBUG_FIRST_CALL
      if (!firstCallFired)
         MessageInterface::ShowMessage(
            "Calling ITRF::CalculateRotationMatrix at epoch %18.12lf; \n", atEpoch.Get());
   #endif
   Real theEpoch = atEpoch.Get();

   //  Perform time computations and read EOP file
   Real sec2rad = GmatMathConstants::RAD_PER_DEG/3600;
   Real a1MJD = theEpoch;

   Real utcMJD = TimeConverterUtil::Convert(a1MJD,
                    TimeConverterUtil::A1MJD, TimeConverterUtil::UTCMJD,
                    JD_JAN_5_1941);
   Real offset = JD_JAN_5_1941 - JD_NOV_17_1858;

   Real xp,yp,LOD,dUT1;
   dUT1 = eop->GetUt1UtcOffset(utcMJD +  offset);
   eop->GetPolarMotionAndLod(utcMJD +  offset, xp, yp, LOD);

   xp = xp*sec2rad;
   yp = yp*sec2rad;

   Real ut1MJD = TimeConverterUtil::Convert(a1MJD,
                    TimeConverterUtil::A1MJD, TimeConverterUtil::UT1,
                    JD_JAN_5_1941);

   // Compute elapsed Julian centuries (UT1)
   Real tDiff = JD_JAN_5_1941 - JD_OF_J2000;
   Real jdUT1 = ut1MJD + JD_JAN_5_1941;

   // convert input A1 MJD to TT MJD (for most calculations)
   Real ttMJD = TimeConverterUtil::Convert(a1MJD,
                   TimeConverterUtil::A1MJD, TimeConverterUtil::TTMJD,
                   JD_JAN_5_1941);

   Real jdTT    = ttMJD + JD_JAN_5_1941; // right?
   // Compute Julian centuries of TDB from the base epoch (J2000)
   // NOTE - this is really TT, an approximation of TDB *********
   Real T_TT    = (ttMJD + tDiff) / DAYS_PER_JULIAN_CENTURY;

   //  Compute the Polar Motion Matrix, W, and Earth Rotation Angle, theta
   Real sPrime = -0.000047*sec2rad*T_TT;
   Rmatrix33 W = R3(-sPrime)*R2(xp)*R1(yp);
   Real theta  = fmod(GmatMathConstants::TWO_PI*(0.7790572732640 + 1.00273781191135448*(jdUT1 - 2451545.0)),GmatMathConstants::TWO_PI);

   //  Compute the precession-nutation matrix
   //  . interpolate the XYs data file
   Real data[3];
   if (iauFile == NULL)
   {
      throw CoordinateSystemException("Error: IAUFile object is NULL. GMAT cannot get IAU data.\n");
   }
   iauFile->GetIAUData(jdTT,data,3,9);
   Real X = data[0]*sec2rad;
   Real Y = data[1]*sec2rad;
   Real s = data[2]*sec2rad;

   // . construct the Precession Nutation matrix
   Real b = 1/(1 + sqrt(1- X*X - Y*Y));
   Rmatrix33 CT;
   CT.SetElement(0,0, 1-b*X*X);	CT.SetElement(0,1, -b*X*Y ); CT.SetElement(0,2,X);
   CT.SetElement(1,0, -b*X*Y);   CT.SetElement(1,1, 1-b*Y*Y); CT.SetElement(1,2,Y);
   CT.SetElement(2,0, -X);       CT.SetElement(2,1, -Y);      CT.SetElement(2,2,(1 - b*(X*X + Y*Y)));
   CT = CT*R3(s);

   //  Form the complete rotation matrix from ITRF to GCRF
   Rmatrix33 R    = CT*R3(-theta)*W;
   Real omegaEarth = 7.292115146706979e-5*(1 - LOD/86400);
   Rvector3 vec(0.0, 0.0, omegaEarth);
   Rmatrix33 Rdot = CT*R3(-theta)*Skew(vec)*W;
   rotMatrix = R;
   rotDotMatrix = Rdot;

   #ifdef DEBUG_ITRF_ROT_MATRIX
      MessageInterface::ShowMessage("a1MJD  = %18.10lf\n",a1MJD);
      MessageInterface::ShowMessage("utcMJD = %18.10lf\n",utcMJD);
      MessageInterface::ShowMessage("dUT1=%18.10e, xp=%18.10e, yp=%18.10e, LOD=%18.10e\n",dUT1,xp,yp,LOD);
      MessageInterface::ShowMessage("ut1MJD = %18.10lf\n",ut1MJD);
      MessageInterface::ShowMessage("ttMJD  = %18.10lf\n",ttMJD);
      MessageInterface::ShowMessage("jdTT   = %18.10lf\n",jdTT);
      MessageInterface::ShowMessage("jdUT1  = %18.10lf\n",jdUT1);
      MessageInterface::ShowMessage("T_TT   = %18.10lf\n\n",T_TT);

      MessageInterface::ShowMessage("sPrime = %18.10lf,  theta = %18.10lf\n",sPrime, theta);
      MessageInterface::ShowMessage("W(0,0)=%18.10lf,  W(0,1)=%18.10lf,  W(0,2)=%18.10lf\n",W.GetElement(0,0),W.GetElement(0,1),W.GetElement(0,2));
      MessageInterface::ShowMessage("W(1,0)=%18.10lf,  W(1,1)=%18.10lf,  W(1,2)=%18.10lf\n",W.GetElement(1,0),W.GetElement(1,1),W.GetElement(1,2));
      MessageInterface::ShowMessage("W(2,0)=%18.10lf,  W(2,1)=%18.10lf,  W(2,2)=%18.10lf\n",W.GetElement(2,0),W.GetElement(2,1),W.GetElement(2,2));

      MessageInterface::ShowMessage("X = %18.10lf,   Y = %18.10lf,   s = %18.10lf\n", X, Y, s);
      MessageInterface::ShowMessage("CT(0,0)=%18.10lf,  CT(0,1)=%18.10lf,  CT(0,2)=%18.10lf\n",CT.GetElement(0,0),CT.GetElement(0,1),CT.GetElement(0,2));
      MessageInterface::ShowMessage("CT(1,0)=%18.10lf,  CT(1,1)=%18.10lf,  CT(1,2)=%18.10lf\n",CT.GetElement(1,0),CT.GetElement(1,1),CT.GetElement(1,2));
      MessageInterface::ShowMessage("CT(2,0)=%18.10lf,  CT(2,1)=%18.10lf,  CT(2,2)=%18.10lf\n",CT.GetElement(2,0),CT.GetElement(2,1),CT.GetElement(2,2));

      MessageInterface::ShowMessage("R(0,0)=%18.10lf,  R(0,1)=%18.10lf,  R(0,2)=%18.10lf\n",R.GetElement(0,0),R.GetElement(0,1),R.GetElement(0,2));
      MessageInterface::ShowMessage("R(1,0)=%18.10lf,  R(1,1)=%18.10lf,  R(1,2)=%18.10lf\n",R.GetElement(1,0),R.GetElement(1,1),R.GetElement(1,2));
      MessageInterface::ShowMessage("R(2,0)=%18.10lf,  R(2,1)=%18.10lf,  R(2,2)=%18.10lf\n",R.GetElement(2,0),R.GetElement(2,1),R.GetElement(2,2));

      MessageInterface::ShowMessage("Rdot(0,0)=%18.10lf,  Rdot(0,1)=%18.10lf,  Rdot(0,2)=%18.10lf\n",Rdot.GetElement(0,0),Rdot.GetElement(0,1),Rdot.GetElement(0,2));
      MessageInterface::ShowMessage("Rdot(1,0)=%18.10lf,  Rdot(1,1)=%18.10lf,  Rdot(1,2)=%18.10lf\n",Rdot.GetElement(1,0),Rdot.GetElement(1,1),Rdot.GetElement(1,2));
      MessageInterface::ShowMessage("Rdot(2,0)=%18.10lf,  Rdot(2,1)=%18.10lf,  Rdot(2,2)=%18.10lf\n\n\n",Rdot.GetElement(2,0),Rdot.GetElement(2,1),Rdot.GetElement(2,2));
   #endif

   #ifdef DEBUG_FIRST_CALL
      firstCallFired = true;
      MessageInterface::ShowMessage("NOW exiting ITRFAxes::CalculateRotationMatrix ...\n");
   #endif

}