C++ Rvector3类代码示例

//------------------------------------------------------------------------------
Real AngleUtil::ComputeAngleInDeg(const Rvector3 &vecA, const Rvector3 &vecB,
                                  Real tol)
{
   Rvector3 uvecA = vecA.GetUnitVector();
   Rvector3 uvecB = vecB.GetUnitVector();
   Real aDotB = uvecA * uvecB;
   Real angRad = 0.0;
   Real angDeg = 0.0;
   
   // if dot-product is less than or equal to tolerance, the angle is arccos of
   // the dot-product, otherwise, the angle is arcsin of the magnitude of the
   // cross-product.
   
   if (Abs(aDotB) <= Abs(tol))
   {
      angRad = ACos(aDotB);
      angDeg = DEG_PER_RAD * angRad;
   }
   else
   {
      // compute cross-product of two vectors
      Rvector3 aCrossB = Cross(uvecA, uvecB);
      Real crossMag = aCrossB.GetMagnitude();
      angRad = ASin(crossMag);

      if (aDotB < 0.0)
      {
         angRad = PI_OVER_TWO - angRad;
         angDeg = DEG_PER_RAD * angRad;
      }
   }
   
   return angDeg;
}

//------------------------------------------------------------------------
RealArray PhysicalMeasurement::TroposphereCorrection(Real freq, Rvector3 rVec, Rmatrix Ro_j2k)
{
   RealArray tropoCorrection;

   if (troposphere != NULL)
   {
   	Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6);
   	troposphere->SetWaveLength(wavelength);
   	Real elevationAngle = asin((Ro_j2k*rVec.GetUnitVector()).GetElement(2));
   	troposphere->SetElevationAngle(elevationAngle);
   	troposphere->SetRange(rVec.GetMagnitude()*GmatMathConstants::KM_TO_M);
   	tropoCorrection = troposphere->Correction();
//   	Real rangeCorrection = tropoCorrection[0]/GmatMathConstants::KM_TO_M;

		#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
			MessageInterface::ShowMessage("       Apply Troposphere media correction:\n");
			MessageInterface::ShowMessage("         .Wave length = %.12lf m\n", wavelength);
			MessageInterface::ShowMessage("         .Elevation angle = %.12lf degree\n", elevationAngle*GmatMathConstants::DEG_PER_RAD);
			MessageInterface::ShowMessage("         .Range correction = %.12lf m\n", tropoCorrection[0]);
		#endif
   }
   else
   {
   	tropoCorrection.push_back(0.0);
   	tropoCorrection.push_back(0.0);
   	tropoCorrection.push_back(0.0);
   }

   return tropoCorrection;
}

Rmatrix33 NadirPointing::TRIAD(Rvector3& V1, Rvector3& V2, Rvector3& W1, Rvector3& W2)
{
	// V1, V2 : defined in frame A
	// W1, W2 : defined in frame B
	// TRIAD algorithm calculates the rotation matrix from A to B

	Rvector3 r1 = V1.GetUnitVector();
	Rvector3 temp = Cross(V1,V2);
	Rvector3 r2 = temp.GetUnitVector();
	Rvector3 r3 = Cross(V1,temp);
	r3 = r3.GetUnitVector();

	Rvector3 s1 = W1.GetUnitVector();
	temp = Cross(W1,W2);
	Rvector3 s2 = temp.GetUnitVector();
	Rvector3 s3 = Cross(W1,temp);
	s3 = s3.GetUnitVector();

	//MessageInterface::LogMessage("s3 = %f\t%f\t%f\n", s3(0), s3(1), s3(2) );

	// YRL, calculate resultRotMatrix
	Rmatrix33 resultRotMatrix = Outerproduct(s1,r1) + Outerproduct(s2,r2) + Outerproduct(s3,r3) ;	

	return resultRotMatrix;
}

//------------------------------------------------------------------------------
bool Periapsis::Evaluate()
{
   if (mOrigin == NULL)
      OrbitData::InitializeRefObjects();
   
   Rvector6 cartState = OrbitData::GetRelativeCartState(mOrigin);
   
   //MessageInterface::ShowMessage
   //   (wxT("===> Periapsis::Evaluate() mOrigin=%s, cartState=%s\n"),
   //    mOrigin->GetName().c_str(), cartState.ToString().c_str());
   
   if (cartState == Rvector6::RVECTOR6_UNDEFINED)
      return false;
   
   // compute position and velocity unit vectors
   Rvector3 pos = Rvector3(cartState[0], cartState[1], cartState[2]);
   Rvector3 vel = Rvector3(cartState[3], cartState[4], cartState[5]);
   Rvector3 R = pos.GetUnitVector();
   Rvector3 V = vel.GetUnitVector();

   // compute cos(90 - beta) as the dot product of the R and V vectors
//    Real rdotv = R*V;
//    mRealValue = rdotv;
   mRealValue = R*V;
   // Changed to use IsEqual() (LOJ: 2010.02.02)
   //if (mRealValue == 0.0)
   if (GmatMathUtil::IsEqual(mRealValue, 0.0))
      mRealValue = 1.0e-40;

   return true;
}

//------------------------------------------------------------------------------
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;
}

//--------------------------------------------------------------------
void Structure::CalcCenter ()
// Find the center of the object
   {
   ZMinMax minmax;
   for (Integer ix=0;  ix<=Appendages.Size()-1;  ++ix)
      minmax.Broaden(Appendages[ix]->Body.MinMax());
   for (int i=0;  i<=3-1;  ++i)
      Center[i] = (minmax.Min.V[i]+minmax.Max.V[i])/2;
   Rvector3 size;
   for (int i=0;  i<=3-1;  ++i)
      //size[i] = std::max(Center[i]-minmax.Min.V[i],minmax.Max.V[i]-Center[i]);
      size[i] = GmatMathUtil::Max(Center[i]-minmax.Min.V[i],minmax.Max.V[i]-Center[i]);
   Radius = size.GetMagnitude();
   }

// Inner function to rotate a vector around another vector by angle
Rvector3 Camera::RotateAround(Rvector3 vector, Real angle, Rvector3 axis){
   // Store the old x,y,z values
   Real oldX = vector[0], oldY = vector[1], oldZ = vector[2];
   // Store the cos and sin values, for convenience
   Real c = cos(angle), s = sin(angle);
   // Store the x,y,z values of the axis, for convenience
   axis.Normalize();
   Real x = axis[0], y = axis[1], z = axis[2];
   // Rotate the vector around the axis
   vector.Set((1+(1-c)*(x*x-1))*oldX + (-z*s+(1-c)*x*y)*oldY + (y*s+(1-c)*x*z)*oldZ,
      (z*s+(1-c)*x*y)*oldX + (1+(1-c)*(y*y-1))*oldY + (-x*s+(1-c)*y*z)*oldZ,
      (-y*s+(1-c)*x*z)*oldX + (x*s+(1-c)*y*z)*oldY + (1+(1-c)*(z*z-1))*oldZ);
   // Make sure the direction vectors are still orthogonal to each other
   //ReorthogonalizeVectors();
   return vector;
}

//--------------------------------------------------------------------
void SurfaceMesh::BuildNormals ()
// Create the normal vectors for the faces
   {
   for (Integer i=0;  i<=FacesSize-1;  ++i)
      {
      ZFace& face (Faces[i]);
      Rvector3 x = Vectors[face.VertexIndex[0]].WrtBody.ConvertToRvector3();
      Rvector3 y = Vectors[face.VertexIndex[1]].WrtBody.ConvertToRvector3();
      Rvector3 z = Vectors[face.VertexIndex[2]].WrtBody.ConvertToRvector3();
      Rvector3 xy = y-x;
      Rvector3 yz = z-y;
      Rvector3 n = Cross(xy,yz);
      if (n.GetMagnitude() != 0)
         n = n.Normalize();
      int index = MakeVector (n,false);
      for (Integer j=0;  j<=3-1;  ++j)
         face.NormalIndex[j] = index;
      }
   }

//-------------------------------------------------------------------------------
bool PolyhedronBody::FaceNormals()
{
	PolygonFace face;
   Rvector3 n;
	Rvector3 r1, r2, A, B, C;

	fn.clear();

	// Create normal vector list based on facesList:
	Real x, y, z;
	for (UnsignedInt i = 0; i < facesList.size(); ++i)
	{
		// Get indices of triangle ith:
		face = facesList[i];
		A.Set(verticesList[face[0]].Get(0), verticesList[face[0]].Get(1), verticesList[face[0]].Get(2));		// vertex A of triangle ABC
		B.Set(verticesList[face[1]].Get(0), verticesList[face[1]].Get(1), verticesList[face[1]].Get(2));		// vertex B of triangle ABC
		C.Set(verticesList[face[2]].Get(0), verticesList[face[2]].Get(1), verticesList[face[2]].Get(2));		// vertex C of triangle ABC
		r1 = B - A;						// vector AB
		r2 = C - B;						// vector BC
		x = r1[1]*r2[2] - r1[2]*r2[1];
		y = r1[2]*r2[0] - r1[0]*r2[2];
		z = r1[0]*r2[1] - r1[1]*r2[0];
      n.Set(x,y,z);		         // n = AB x BC
		if (n.Norm() < 1.0e-15)
			return false;

		n.Normalize();					// normal vector of triangle ABC
		fn.push_back(n);				// add normal vector to the normal vectors list
	}

#ifdef DEBUG_FACENORMALS_CALCULATION
	MessageInterface::ShowMessage("List of normal vectors:\n");
	for (UnsignedInt i = 0; i < fn.size(); ++i)
	{
		MessageInterface::ShowMessage("%.15lf   %.15lf   %.15lf\n", fn[i][0], fn[i][1], fn[i][2]);
	}
#endif
	return true;
}

//---------------------------------------------------------------------------
// Real Ionosphere::BendingAngle()
//---------------------------------------------------------------------------
Real Ionosphere::BendingAngle()
{
   Rvector3 rangeVec = spacecraftLoc - stationLoc;
   Rvector3 dR = rangeVec / NUM_OF_INTERVALS;
   Rvector3 p1 = stationLoc;
   Rvector3 p2, delta;
   Real n1, n2, dn_drho, de1, de2, integrant;
   Real gammar = 0.0;
   
   //Real beta0 = GmatConstants::PI/2 - acos(rangeVec.GetUnitVector()*p1.GetUnitVector());
   Real beta0 = GmatMathConstants::PI_OVER_TWO - acos(rangeVec.GetUnitVector()*p1.GetUnitVector());
   //MessageInterface::ShowMessage("Elevation angle = %f\n", beta0*180/GmatConstants::PI);
   MessageInterface::ShowMessage("Elevation angle = %f\n", beta0*GmatMathConstants::DEG_PER_RAD);
   Real beta = beta0;
   Real freq = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / waveLength;
   for(int i = 0; i < NUM_OF_INTERVALS; ++i)
   {
      p2 = p1 + dR;
      
      delta = dR/100;
      de1 = ElectronDensity(p1+delta, p1);
      de2 = ElectronDensity(p1+2*delta,p1+delta);
      n1 = 1 - 40.3*de1/(freq*freq);
      n2 = 1 - 40.3*de2/(freq*freq);
      dn_drho = -40.3*(de2 - de1)/ (freq*freq) / (((p1+delta).GetMagnitude() - p1.GetMagnitude())*GmatMathConstants::KM_TO_M);
      integrant = dn_drho/(n1*tan(beta));
      gammar += integrant*(p2.GetMagnitude() - p1.GetMagnitude())*GmatMathConstants::KM_TO_M;
      //MessageInterface::ShowMessage("de1 = %.12lf,  de2 = %.12lf, rho1 = %f,   "
      //"rho2 = %f, integrant = %.12lf, gammar =%.12lf\n",de1, de2, p1.GetMagnitude(),
      //p2.GetMagnitude(), integrant, gammar*180/GmatConstants::PI);
      
      p1 = p2;
      beta = beta0 + gammar;
   }
   
   return gammar;
}

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;

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

//------------------------------------------------------------------------------
Integer CoordUtil::ComputeCartToKepl(Real grav, Real r[3], Real v[3], Real *tfp,
                                     Real elem[6], Real *ma)
{
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(wxT("CoordUtil::ComputeCartToKepl called ... \n"));
      MessageInterface::ShowMessage(wxT("   grav = %12.10f\n"), grav);
      MessageInterface::ShowMessage(wxT("   KEP_ECC_TOL = %12.10f\n"), GmatOrbitConstants::KEP_ECC_TOL);
   #endif

   if (Abs(grav) < 1E-30)
      return(2);
   
   Rvector3 pos(r[0], r[1], r[2]);
   Rvector3 vel(v[0], v[1], v[2]);
   
   // eqn 4.1
   Rvector3 angMomentum = Cross(pos, vel);
   
   // eqn 4.2
   Real h = angMomentum.GetMagnitude();
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, pos = %12.10f  %12.10f  %12.10f \n"), pos[0], pos[1], pos[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, vel = %12.10f  %12.10f  %12.10f \n"), vel[0], vel[1], vel[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, angMomentum = %12.10f  %12.10f  %12.10f\n"),
            angMomentum[0], angMomentum[1], angMomentum[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, h = %12.10f\n"), h);
   #endif
   
//   if (h < 1E-30)
//   {
//      return 1;
//   }
   
   // eqn 4.3
   Rvector3 nodeVec = Cross(Rvector3(0,0,1), angMomentum);
   
   // eqn 4.4
   Real n = nodeVec.GetMagnitude();
   
   // eqn 4.5 - 4.6
   Real posMag = pos.GetMagnitude();
   Real velMag = vel.GetMagnitude();
   
   // eqn 4.7 - 4.8
   Rvector3 eccVec = (1/grav)*((velMag*velMag - grav/posMag)*pos - (pos * vel) * vel);
   Real e = eccVec.GetMagnitude();
   
   // eqn 4.9
   Real zeta = 0.5*velMag*velMag - grav/posMag;
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, nodeVec = %12.10f  %12.10f  %12.10f \n"),
            nodeVec[0], nodeVec[1], nodeVec[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, n = %12.10f\n"), n);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, posMag = %12.10f\n"), posMag);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, velMag = %12.10f\n"), velMag);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, eccVec = %12.10f  %12.10f  %12.10f \n"), eccVec[0], eccVec[1], eccVec[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, e = %12.10f\n"), e);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, zeta = %12.10f\n"), zeta);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, Abs(1.0 - e) = %12.10f\n"), Abs(1.0 - e));
   #endif
   
   if ((Abs(1.0 - e)) <= GmatOrbitConstants::KEP_ECC_TOL)
   {
      wxString errmsg = wxT("Error in conversion to Keplerian state: ");
      errmsg += wxT("The state results in an orbit that is nearly parabolic.\n");
      throw UtilityException(errmsg);
   } 
   
   // eqn 4.10
   Real sma = -grav/(2*zeta);
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, sma = %12.10f\n"), sma);
   #endif


   if (Abs(sma*(1 - e)) < .001)
   {
      throw UtilityException
      (wxT("Error in conversion from Cartesian to Keplerian state: ")
       wxT("The state results in a singular conic section with radius of periapsis less than 1 m.\n"));
//         (wxT("CoordUtil::CartesianToKeplerian() ")
//          wxT("Warning: A nearly singular conic section was encountered while ")
//          wxT("converting from the Cartesian state to the Keplerian elements.  The radius of ")
//          wxT("periapsis must be greater than 1 meter.\n"));

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

//------------------------------------------------------------------------------
bool USNTwoWayRange::Evaluate(bool withEvents)
{
   bool retval = false;

   if (!initialized)
      InitializeMeasurement();

   #ifdef DEBUG_RANGE_CALC
      MessageInterface::ShowMessage("Entered USNTwoWayRange::Evaluate()\n");
      MessageInterface::ShowMessage("  ParticipantCount: %d\n",
            participants.size());
   #endif

   if (withEvents == false)
   {
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("USN 2-Way Range Calculation without "
               "events\n");
      #endif

      #ifdef VIEW_PARTICIPANT_STATES
         DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
               "Evaluating USN 2-Way Range without events");
      #endif

      CalculateRangeVectorInertial();
      Rvector3 outState;

      // Set feasibility off of topocentric horizon, set by the Z value in topo
      // coords
      std::string updateAll = "All";
      UpdateRotationMatrix(currentMeasurement.epoch, updateAll);
      outState = R_o_j2k * rangeVecInertial;
      currentMeasurement.feasibilityValue = outState[2];

      #ifdef CHECK_PARTICIPANT_LOCATIONS
         MessageInterface::ShowMessage("Evaluating without events\n");
         MessageInterface::ShowMessage("Calculating USN 2-Way Range at epoch "
               "%.12lf\n", currentMeasurement.epoch);
         MessageInterface::ShowMessage("   J2K Location of %s, id = '%s':  %s",
               participants[0]->GetName().c_str(),
               currentMeasurement.participantIDs[0].c_str(),
               p1Loc.ToString().c_str());
         MessageInterface::ShowMessage("   J2K Location of %s, id = '%s':  %s",
               participants[1]->GetName().c_str(),
               currentMeasurement.participantIDs[1].c_str(),
               p2Loc.ToString().c_str());
         Rvector3 bfLoc = R_o_j2k * p1Loc;
         MessageInterface::ShowMessage("   BodyFixed Location of %s:  %s",
               participants[0]->GetName().c_str(),
               bfLoc.ToString().c_str());
         bfLoc = R_o_j2k * p2Loc;
         MessageInterface::ShowMessage("   BodyFixed Location of %s:  %s\n",
               participants[1]->GetName().c_str(),
               bfLoc.ToString().c_str());
      #endif

      if (currentMeasurement.feasibilityValue > 0.0)
      {
         currentMeasurement.isFeasible = true;
         currentMeasurement.value[0] = rangeVecInertial.GetMagnitude();
         currentMeasurement.eventCount = 2;

         retval = true;
      }
      else
      {
         currentMeasurement.isFeasible = false;
         currentMeasurement.value[0] = 0.0;
         currentMeasurement.eventCount = 0;
      }

      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("Calculating Range at epoch %.12lf\n",
               currentMeasurement.epoch);
         MessageInterface::ShowMessage("   Location of %s, id = '%s':  %s",
               participants[0]->GetName().c_str(),
               currentMeasurement.participantIDs[0].c_str(),
               p1Loc.ToString().c_str());
         MessageInterface::ShowMessage("   Location of %s, id = '%s':  %s",
               participants[1]->GetName().c_str(),
               currentMeasurement.participantIDs[1].c_str(),
               p2Loc.ToString().c_str());
         MessageInterface::ShowMessage("   Range Vector:  %s\n",
               rangeVecInertial.ToString().c_str());
         MessageInterface::ShowMessage("   R(Groundstation) dot RangeVec =  %lf\n",
               currentMeasurement.feasibilityValue);
         MessageInterface::ShowMessage("   Feasibility:  %s\n",
               (currentMeasurement.isFeasible ? "true" : "false"));
         MessageInterface::ShowMessage("   Range is %.12lf\n",
               currentMeasurement.value[0]);
         MessageInterface::ShowMessage("   EventCount is %d\n",
               currentMeasurement.eventCount);
      #endif

      #ifdef SHOW_RANGE_CALC
         MessageInterface::ShowMessage("Range at epoch %.12lf is ",
               currentMeasurement.epoch);
         if (currentMeasurement.isFeasible)
//.........这里部分代码省略.........

//---------------------------------------------------------------------------
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;
   }
//.........这里部分代码省略.........

//---------------------------------------------------------------------------
float Ionosphere::ElectronDensity(Rvector3 pos2, Rvector3 pos1)
{
   // the fisrt position's latitude and longitude (unit: degree):
   //real latitude = (real)(GmatMathConstants::PI_OVER_TWO_DEG - acos(pos1.Get(2)/pos1.GetMagnitude())*GmatMathConstants::DEG_PER_RAD);	// unit: degree
   real latitude = (real)(asin(pos1.Get(2)/pos1.GetMagnitude())*GmatMathConstants::DEG_PER_RAD);	// unit: degree
   real longitude = (real)(atan2(pos1.Get(1),pos1.Get(0))*GmatMathConstants::DEG_PER_RAD);			// unit: degree
   
   // mmag  = 0 geographic   =1 geomagnetic coordinates
   integer jmag = 0;	// 1;
   
   // jf(1:30)     =.true./.false. flags; explained in IRISUB.FOR
   logical jf[31];
   for (int i=1; i <= 30; ++i)
      jf[i] = TRUE_;
   
   jf[5] = FALSE_;
   jf[6] = FALSE_;
   jf[23] = FALSE_;
   jf[29] = FALSE_;
   jf[30] = FALSE_;
   
//   jf[21] = FALSE_;
//   jf[28] = FALSE_;
   
   // iy,md        date as yyyy and mmdd (or -ddd)
   // hour         decimal hours LT (or UT+25)
   integer iy = (integer)yyyy;
   integer md = (integer)mmdd;
   real hour = (real)hours;
   
   // Upper and lower integration limits
   real hbeg = (real)(pos1.GetMagnitude() - earthRadius); // 0
   real hend = hbeg;
   real hstp = 1.0;
   
   integer error = 0;

   real outf[20*501+1];
   real oarr[51];


//   iri_sub(&jf[1], &jmag, &latitude, &longitude, &iy, &md, &hour,
//           &hbeg, &hend, &hstp, &outf[21], &oarr[1], &error);

   integer ivar = 1;		// get attitude result
   integer iut = 1;			// 1 for universal time; 0 for local time

# ifdef DEBUG_IONOSPHERE_ELECT_DENSITY
   MessageInterface::ShowMessage("           .At time = %lf A1Mjd:",epoch);
   MessageInterface::ShowMessage("         year = %d   md = %d   hour = %lf h,   time type = %s,\n", iy, md, hour, (iut?"Universal":"Local"));
   MessageInterface::ShowMessage("              At position (x,y,z) = (%lf,  %lf,  %lf)km in Earth fixed coordinate system: ", pos1[0], pos1[1], pos1[2]);
   MessageInterface::ShowMessage("(latitude = %lf degree,  longitude = %lf degree,  attitude = %lf km,  ", latitude, longitude, hbeg);
   MessageInterface::ShowMessage("coordinate system type = %s)\n",(jmag?"Geomagetic":"Geographic"));
#endif
   iri_web(&jmag, &jf[1], &latitude, &longitude, &iy, &md, &iut, &hour, &hbeg, &hbeg, 
	   &ivar, &hbeg, &hend, &hstp, &outf[21], &oarr[1], &error);

   if (error != 0)
   {
      MessageInterface::ShowMessage("Ionosphere data files not found\n");
      throw new MeasurementException("Ionosphere data files not found\n");
   }

   real density = outf[20+1];
   if (density < 0)
      density = 0;

#ifdef DEBUG_IONOSPHERE_ELECT_DENSITY
   MessageInterface::ShowMessage("              Electron density at that time and location = %le electrons per m3.\n", density);
#endif

   return density;         //*(pos2-pos1).GetMagnitude();
}