//------------------------------------------------------------------------
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;
}
//------------------------------------------------------------------------------
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;
}
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;
}
//------------------------------------------------------------------------------
bool Apoapsis::Evaluate()
{
if (mOrigin == NULL)
OrbitData::InitializeRefObjects();
Rvector6 cartState = OrbitData::GetRelativeCartState(mOrigin);
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;
if (mRealValue == 0.0)
mRealValue = -1.0e-40;
//MessageInterface::ShowMessage(wxT("Apoapsis::Evaluate() r=%f,%f,%f, v=%f,%f,%f, r.v=%f\n"),
// R[0], R[1], R[2], V[0], V[1], V[2], rdotv);
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 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);
}
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;
constraintVector[0] = 0;
constraintVector[1] = 1;
constraintVector[2] = 0;
}
else if ( modeOfConstraint == "VelocityConstraint" )
{
referenceVector[0] = -1;
referenceVector[1] = 0;
referenceVector[2] = 0;
constraintVector[0] = 0;
constraintVector[1] = 0;
constraintVector[2] = 1;
}
// RBi calculation using TRIAD (from LVLH to body frame)
Rmatrix33 RiB = TRIAD( bodyAlignmentVector, bodyConstraintVector, referenceVector, constraintVector );
// the rotation matrix (from body to inertial)
dcm = RIi*RiB;
// the final rotation matrix (from inertial to body)
dcm = dcm.Transpose();
// We do NOT calculate angular velocity for Nadir.
// TODO : Throw AttitudeException??
/*
Rmatrix33 wxIBB = - RBi*RiIDot*dcm.Transpose();
Rvector3 wIBB;
wIBB.Set(wxIBB(2,1),wxIBB(0,2),wxIBB(1,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"));
}
// eqn 4.11
Real i = ACos( angMomentum.Get(2)/h );
if (i >= PI - GmatOrbitConstants::KEP_TOL)
{
throw UtilityException
(wxT("Error in conversion to Keplerian state: ")
wxT("GMAT does not currently support orbits with inclination of 180 degrees.\n"));
}
Real raan, argPeriapsis, trueAnom;
raan=argPeriapsis=trueAnom=0;
if ( e >= 1E-11 && i >= 1E-11 ) // CASE 1: Non-circular, Inclined Orbit
{
raan = ACos( nodeVec.Get(0)/n );
if (nodeVec.Get(1) < 0)
raan = TWO_PI - raan;
argPeriapsis = ACos( (nodeVec*eccVec)/(n*e) );
if (eccVec.Get(2) < 0)
argPeriapsis = TWO_PI - argPeriapsis;
trueAnom = ACos( (eccVec*pos)/(e*posMag) );
if (pos*vel < 0)
trueAnom = TWO_PI - trueAnom;
}
if ( e >= 1E-11 && i < 1E-11 ) // CASE 2: Non-circular, Equatorial Orbit
{
raan = 0;
argPeriapsis = ACos(eccVec.Get(0)/e);
if (eccVec.Get(1) < 0)
argPeriapsis = TWO_PI - argPeriapsis;
trueAnom = ACos( (eccVec*pos)/(e*posMag) );
if (pos*vel < 0)
trueAnom = TWO_PI - trueAnom;
}
if ( e < 1E-11 && i >= 1E-11 ) // CASE 3: Circular, Inclined Orbit
{
raan = ACos( nodeVec.Get(0)/n );
if (nodeVec.Get(1) < 0)
raan = TWO_PI - raan;
argPeriapsis = 0;
trueAnom = ACos( (nodeVec*pos)/(n*posMag) );
if (pos.Get(2) < 0)
trueAnom = TWO_PI - trueAnom;
}
if ( e < 1E-11 && i < 1E-11 ) // CASE 4: Circular, Equatorial Orbit
{
raan = 0;
argPeriapsis = 0;
trueAnom = ACos(pos.Get(0)/posMag);
if (pos.Get(1) < 0)
trueAnom = TWO_PI - trueAnom;
}
elem[0] = sma;
elem[1] = e;
elem[2] = i*DEG_PER_RAD;
elem[3] = raan*DEG_PER_RAD;
elem[4] = argPeriapsis*DEG_PER_RAD;
elem[5] = trueAnom*DEG_PER_RAD;
return 0;
}
//------------------------------------------------------------------------------
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)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("USN 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES_WITH_EVENTS
DumpParticipantStates("********************************************\n"
"Evaluating USN 2-Way Range with located events");
#endif
// 1. Get the range from the down link
Rvector3 r1, r2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("r1 = (%f, %f, %f)\n", r1.Get(0), r1.Get(1), r1.Get(2));
MessageInterface::ShowMessage("r2 = (%f, %f, %f)\n", r2.Get(0), r2.Get(1), r2.Get(2));
#endif
Rvector3 downlinkVector = r2 - r1; // rVector = r2 - r1;
downlinkRange = downlinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range = r2-r1: %.12lf km\n",
downlinkRange);
#endif
// 2. Calculate down link range rate:
Rvector3 p1V = downlinkLeg.GetVelocity(participants[0]);
Rvector3 p2V = downlinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("p1V = (%f, %f, %f)\n", p1V.Get(0), p1V.Get(1), p1V.Get(2));
MessageInterface::ShowMessage("p2V = (%f, %f, %f)\n", p2V.Get(0), p2V.Get(1), p2V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector downRRateVec = p2V - p1V /* - r12_j2k_vel*/;
Rvector3 rangeUnit = downlinkVector.GetUnitVector();
downlinkRangeRate = downRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range Rate: %.12lf km/s\n",
downlinkRangeRate);
#endif
// 3. Get the transponder delay
targetDelay = GetDelay(1,0);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(
" USN Transponder delay for %s = %.12lf s\n",
participants[1]->GetName().c_str(), targetDelay);
#endif
// 4. Get the range from the uplink
Rvector3 r3, r4;
r3 = uplinkLeg.GetPosition(participants[0]);
r4 = uplinkLeg.GetPosition(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("r3 = (%f, %f, %f)\n", r3.Get(0), r3.Get(1), r3.Get(2));
MessageInterface::ShowMessage("r4 = (%f, %f, %f)\n", r4.Get(0), r4.Get(1), r4.Get(2));
#endif
Rvector3 uplinkVector = r4 - r3;
uplinkRange = uplinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range = r4-r3: %.12lf km\n",
uplinkRange);
#endif
// 5. Calculate up link range rate
Rvector3 p3V = uplinkLeg.GetVelocity(participants[0]);
Rvector3 p4V = uplinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("p3V = (%f, %f, %f)\n", p3V.Get(0), p3V.Get(1), p3V.Get(2));
MessageInterface::ShowMessage("p4V = (%f, %f, %f)\n", p4V.Get(0), p4V.Get(1), p4V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector upRRateVec = p4V - p3V /* - r12_j2k_vel*/ ;
rangeUnit = uplinkVector.GetUnitVector();
uplinkRangeRate = upRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range Rate: %.12lf km/s\n",
uplinkRangeRate);
#endif
// 5.1. Target range rate: Do we need this as well?
targetRangeRate = (downlinkRangeRate + uplinkRangeRate) / 2.0;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Target Range Rate: %.12lf km/s\n",
targetRangeRate);
#endif
// 6. Get sensors used in USN 2-ways range
ObjectArray objList1;
ObjectArray objList2;
ObjectArray objList3;
// objList1 := all transmitters in participantHardware list
// objList2 := all receivers in participantHardware list
// objList3 := all transponders in participantHardware list
if (participantHardware.empty()||
((!participantHardware.empty())&&
participantHardware[0].empty()&&
participantHardware[1].empty()
)
)
{
// DO NOT LEAVE THIS TYPE OF MESSAGE IN THE CODE WITHOUT #ifdef WRAPPERS!!!
//MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
Real realRange = (uplinkRange + downlinkRange)/2;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Range = %.12lf km\n", realRange);
#endif
// Set value for currentMeasurement
currentMeasurement.value[0] = realRange;
currentMeasurement.isFeasible = true;
return true;
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[0].begin();
hw != this->participantHardware[0].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transmitter")
objList1.push_back(*hw);
if ((*hw)->GetTypeName() == "Receiver")
objList2.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[1].begin();
hw != this->participantHardware[1].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transponder")
objList3.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
if (objList1.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 transmitter to send signal.\n");
throw new MeasurementException("The first participant does not have only 1 transmitter to send signal.\n");
}
if (objList2.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 receiver to receive signal.\n");
throw new MeasurementException("The first participant does not have only 1 receiver to receive signal.\n");
}
if (objList3.size() != 1)
{
MessageInterface::ShowMessage("The second participant does not have only 1 transponder to transpond signal.\n");
throw new MeasurementException("The second participant does not have only 1 transponder to transpond signal.\n");
}
Transmitter* gsTransmitter = (Transmitter*)objList1[0];
Receiver* gsReceiver = (Receiver*)objList2[0];
Transponder* scTransponder = (Transponder*)objList3[0];
if (gsTransmitter == NULL)
{
MessageInterface::ShowMessage("Transmitter is NULL object.\n");
throw new GmatBaseException("Transmitter is NULL object.\n");
}
if (gsReceiver == NULL)
{
MessageInterface::ShowMessage("Receiver is NULL object.\n");
throw new GmatBaseException("Receiver is NULL object.\n");
}
if (scTransponder == NULL)
{
MessageInterface::ShowMessage("Transponder is NULL object.\n");
throw new GmatBaseException("Transponder is NULL object.\n");
}
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" List of sensors: %s, %s, %s\n",
gsTransmitter->GetName().c_str(), gsReceiver->GetName().c_str(),
scTransponder->GetName().c_str());
#endif
// 7. Get frequency from transmitter of ground station (participants[0])
Signal* uplinkSignal = gsTransmitter->GetSignal();
Real uplinkFreq = uplinkSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" UpLink signal frequency = %.12lf MHz\n", uplinkFreq);
#endif
// 8. Calculate media correction for uplink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Media correction for uplink leg\n");
#endif
Real roundTripTime = ((uplinkRange + downlinkRange)*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)/GmatTimeConstants::SECS_PER_DAY;
// DO NOT LEAVE THIS TYPE OF MESSAGE IN THE CODE WITHOUT #ifdef WRAPPERS!!!
//MessageInterface::ShowMessage("Round trip time = %.12lf\n", roundTripTime);
RealArray uplinkCorrection = CalculateMediaCorrection(uplinkFreq, r1, r2, currentMeasurement.epoch - roundTripTime);
Real uplinkRangeCorrection = uplinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real uplinkRealRange = uplinkRange + uplinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink range correction = %.12lf km\n",uplinkRangeCorrection);
MessageInterface::ShowMessage(" Uplink real range = %.12lf km\n",uplinkRealRange);
#endif
// 9. Doppler shift the frequency from the transmitter using uplinkRangeRate:
Real uplinkDSFreq = (1 - uplinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*uplinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Doppler shift frequency = %.12lf MHz\n", uplinkDSFreq);
#endif
// 10.Set frequency for the input signal of transponder
Signal* inputSignal = scTransponder->GetSignal(0);
inputSignal->SetValue(uplinkDSFreq);
scTransponder->SetSignal(inputSignal, 0);
// 11. Check the transponder feasibility to receive the input signal:
if (scTransponder->IsFeasible(0) == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The transponder is unfeasible to receive uplink signal.\n");
throw new GmatBaseException("The transponder is unfeasible to receive uplink signal.\n");
}
// 12. Get frequency of transponder output signal
Signal* outputSignal = scTransponder->GetSignal(1);
Real downlinkFreq = outputSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink frequency = %.12lf Mhz\n", downlinkFreq);
#endif
// 13. Doppler shift the transponder output frequency by the downlinkRangeRate:
Real downlinkDSFreq = (1 - downlinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*downlinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Doppler shift frequency = %.12lf MHz\n", downlinkDSFreq);
#endif
// 14. Set frequency on receiver
Signal* downlinkSignal = gsReceiver->GetSignal();
downlinkSignal->SetValue(downlinkDSFreq);
// 15. Check the receiver feasibility to receive the downlink signal
if (gsReceiver->IsFeasible() == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
throw new MeasurementException("The receiver is unfeasible to receive downlink signal.\n");
}
// 16. Calculate media correction for downlink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Media correction for downlink leg\n");
#endif
RealArray downlinkCorrection = CalculateMediaCorrection(downlinkDSFreq, r3, r4, currentMeasurement.epoch);
Real downlinkRangeCorrection = downlinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real downlinkRealRange = downlinkRange + downlinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink range correction = %.12lf km\n",downlinkRangeCorrection);
MessageInterface::ShowMessage(" Downlink real range = %.12lf km\n",downlinkRealRange);
#endif
// 17. Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// 18. Calculate real range
Real realRange = uplinkRealRange + downlinkRealRange +
targetDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Calculated real range = %.12lf km\n", realRange/2);
#endif
// 19. Set value for currentMeasurement
currentMeasurement.value[0] = realRange / 2.0;
currentMeasurement.isFeasible = true;
#ifdef PRELIMINARY_DERIVATIVE_CHECK
MessageInterface::ShowMessage("Participants:\n ");
for (UnsignedInt i = 0; i < participants.size(); ++i)
MessageInterface::ShowMessage(" %d: %s of type %s\n", i,
participants[i]->GetName().c_str(),
participants[i]->GetTypeName().c_str());
Integer id = participants[1]->GetType() * 250 +
participants[1]->GetParameterID("CartesianX");
CalculateMeasurementDerivatives(participants[1], id);
#endif
retval = true;
}
return retval;
}
//---------------------------------------------------------------------------
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;
}
else if ((xAxis == "-V") || (xAxis == "-v"))
{
xUnit = -vUnit;
xDot = -vDot;
}
else if ((xAxis == "N") || (xAxis == "n"))
{
xUnit = nUnit;
xDot = nDot;
}
else if ((xAxis == "-N") || (xAxis == "-n"))
{
xUnit = -nUnit;
xDot = -nDot;
}
else
{
xUsed = false;
}
// determine the y-axis
if ((yAxis == "R") || (yAxis == "r"))
{
yUnit = rUnit;
yDot = rDot;
}
else if ((yAxis == "-R") || (yAxis == "-r"))
{
yUnit = -rUnit;
yDot = -rDot;
}
else if ((yAxis == "V") || (yAxis == "v"))
{
yUnit = vUnit;
yDot = vDot;
}
else if ((yAxis == "-V") || (yAxis == "-v"))
{
yUnit = -vUnit;
yDot = -vDot;
}
else if ((yAxis == "N") || (yAxis == "n"))
{
yUnit = nUnit;
yDot = nDot;
}
else if ((yAxis == "-N") || (yAxis == "-n"))
{
yUnit = -nUnit;
yDot = -nDot;
}
else
{
yUsed = false;
}
// determine the z-axis
if ((zAxis == "R") || (zAxis == "r"))
{
zUnit = rUnit;
zDot = rDot;
}
else if ((zAxis == "-R") || (zAxis == "-r"))
{
zUnit = -rUnit;
zDot = -rDot;
}
else if ((zAxis == "V") || (zAxis == "v"))
{
zUnit = vUnit;
zDot = vDot;
}
else if ((zAxis == "-V") || (zAxis == "-v"))
{
zUnit = -vUnit;
zDot = -vDot;
}
else if ((zAxis == "N") || (zAxis == "n"))
{
zUnit = nUnit;
zDot = nDot;
}
else if ((zAxis == "-N") || (zAxis == "-n"))
{
zUnit = -nUnit;
zDot = -nDot;
}
else
{
zUsed = false;
}
// determine the third axis
if (xUsed && yUsed && !zUsed)
{
zUnit = Cross(xUnit, yUnit);
zDot = Cross(xDot, yUnit) + Cross(xUnit, yDot);
}
else if (xUsed && zUsed && !yUsed)
{
yUnit = Cross(zUnit,xUnit);
yDot = Cross(zDot, xUnit) + Cross(zUnit, xDot);
}
else if (yUsed && zUsed && !xUsed)
{
xUnit = Cross(yUnit,zUnit);
xDot = Cross(yDot, zUnit) + Cross(yUnit, zDot);
}
else
{
throw CoordinateSystemException(
"Object referenced axes are improperly defined.");
}
// Compute the rotation matrix
rotMatrix(0,0) = xUnit(0);
rotMatrix(0,1) = yUnit(0);
rotMatrix(0,2) = zUnit(0);
rotMatrix(1,0) = xUnit(1);
rotMatrix(1,1) = yUnit(1);
rotMatrix(1,2) = zUnit(1);
rotMatrix(2,0) = xUnit(2);
rotMatrix(2,1) = yUnit(2);
rotMatrix(2,2) = zUnit(2);
// Compute the rotation derivative matrix
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);
#ifdef DEBUG_ROT_MATRIX
MessageInterface::ShowMessage
("rotMatrix=%s\n", rotMatrix.ToString().c_str());
std::stringstream ss;
ss.setf(std::ios::fixed);
ss.precision(30);
ss << " ----------------- rotMatrix = " << rotMatrix << std::endl;
ss.setf(std::ios::scientific);
ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
#endif
if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
{
std::stringstream errmsg("");
errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
}
}
//---------------------------------------------------------------------------
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();
}
//------------------------------------------------------------------------------
bool TDRSSTwoWayRange::Evaluate(bool withEvents)
{
bool retval = false;
if (!initialized)
InitializeMeasurement();
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Entered TDRSSTwoWayRange::Evaluate()\n");
MessageInterface::ShowMessage(" ParticipantCount: %d\n",
participants.size());
#endif
if (withEvents == false)
{
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation without "
"events\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
"Evaluating TDRSS 2-Way Range without events");
#endif
if (CheckLOS(0, 1, NULL) && CheckLOS(1, 2, NULL))
{
// Calculate the range vector between the groundstation and the TDRS
CalculateRangeVectorInertial(0, 1);
Rvector3 outState;
// Set feasibility off of topospheric 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 TDRSS 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 = 4;
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 (inertial): %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)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("********************************************\n"
"Evaluating TDRSS 2-Way Range with located events");
MessageInterface::ShowMessage("Calculating TDRSS 2-Way Range at epoch "
"%.12lf\n", currentMeasurement.epoch);
#endif
// Get the range from the downlink
Rvector3 r1, r2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
Rvector3 rVector = r2 - r1;
Real realRange = rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at downlink: %.12lf %.12lf %.12lf\n",
participants[0]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at downlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Downlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
r1 = backlinkLeg.GetPosition(participants[1]);
r2 = backlinkLeg.GetPosition(participants[2]);
if (CheckSat2SatLOS(r1, r2, NULL) == false)
return false;
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at backlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at backlink: %.12lf %.12lf %.12lf\n",
participants[2]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Backlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
// Add the pseudorange from the transponder delay
targetDelay = GetDelay(2,0);
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" TDRSS target delay is %.12lf\n",
targetDelay);
#endif
realRange += GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M *
targetDelay;
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Delay Range (%le sec delay): "
"%.12lf\n", targetDelay,
(GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/GmatMathConstants::KM_TO_M * targetDelay));
#endif
r1 = forwardlinkLeg.GetPosition(participants[2]);
r2 = forwardlinkLeg.GetPosition(participants[1]);
if (CheckSat2SatLOS(r1, r2, NULL) == false)
return false;
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[2]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" forwardlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
r1 = uplinkLeg.GetPosition(participants[1]);
r2 = uplinkLeg.GetPosition(participants[0]);
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[0]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Uplink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
currentMeasurement.value[0] = realRange / 2.0;
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Calculated Range: %.12lf\n",
currentMeasurement.value[0]);
#endif
retval = true;
}
return retval;
}
//---------------------------------------------------------------------------
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]);
}
//------------------------------------------------------------------------------
bool DSNTwoWayRange::Evaluate(bool withEvents)
{
bool retval = false;
if (!initialized)
InitializeMeasurement();
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Entered DSNTwoWayRange::Evaluate(%s)\n",
(withEvents ? "true" : "false"));
MessageInterface::ShowMessage(" ParticipantCount: %d\n",
participants.size());
#endif
if (withEvents == false)
{
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("DSN 2-Way Range Calculation without "
"events\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
"Evaluating DSN 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 DSN 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;
SetHardwareDelays(false);
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)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("\n\n DSN 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES_WITH_EVENTS
DumpParticipantStates("********************************************\n"
"Evaluating DSN 2-Way Range with located events");
#endif
// 1. Get the range from the down link
Rvector3 r1, r2;
Real t1, t2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
t1 = downlinkLeg.GetEventData((GmatBase*) participants[0]).epoch;
t2 = downlinkLeg.GetEventData((GmatBase*) participants[1]).epoch;
Rmatrix33 mt = downlinkLeg.GetEventData((GmatBase*) participants[0]).rInertial2obj.Transpose();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("1. Get downlink leg range:\n");
MessageInterface::ShowMessage(" Ground station position in FK5: r1 = (%f, %f, %f)km at epoch = %18.12lf\n", r1.Get(0), r1.Get(1), r1.Get(2), t1);
MessageInterface::ShowMessage(" Spacecraft position in FK5 : r2 = (%f, %f, %f)km at epoch = %18.12lf\n", r2.Get(0), r2.Get(1), r2.Get(2), t2);
MessageInterface::ShowMessage(" Transformation matrix from Earth fixed coordinate system to FK5 coordinate system at epoch = %18.12lf:\n", t1);
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(0,0), mt(0,1), mt(0,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(1,0), mt(1,1), mt(1,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(2,0), mt(2,1), mt(2,2));
#endif
Rvector3 downlinkVector = r2 - r1; // rVector = r2 - r1;
downlinkRange = downlinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range = r2-r1: %.12lf km\n",
downlinkRange);
#endif
// 2. Calculate down link range rate:
Rvector3 p1V = downlinkLeg.GetVelocity(participants[0]);
Rvector3 p2V = downlinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("2. Get downlink leg range rate:\n");
MessageInterface::ShowMessage(" Ground station velocity in FK5: p1V = (%f, %f, %f)km/s\n", p1V.Get(0), p1V.Get(1), p1V.Get(2));
MessageInterface::ShowMessage(" Spacecraft velocity in FK5 : p2V = (%f, %f, %f)km/s\n", p2V.Get(0), p2V.Get(1), p2V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector downRRateVec = p2V - p1V /* - r12_j2k_vel*/;
Rvector3 rangeUnit = downlinkVector.GetUnitVector();
downlinkRangeRate = downRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range Rate: %.12lf km/s\n",
downlinkRangeRate);
#endif
// 3. Get the range from the uplink
Rvector3 r3, r4;
Real t3, t4;
r3 = uplinkLeg.GetPosition(participants[0]);
r4 = uplinkLeg.GetPosition(participants[1]);
t3 = uplinkLeg.GetEventData((GmatBase*) participants[0]).epoch;
t4 = uplinkLeg.GetEventData((GmatBase*) participants[1]).epoch;
Rmatrix33 mt1 = uplinkLeg.GetEventData((GmatBase*) participants[0]).rInertial2obj.Transpose();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("3. Get uplink leg range:\n");
MessageInterface::ShowMessage(" Spacecraft position in FK5 : r4 = (%f, %f, %f)km at epoch = %18.12lf\n", r4.Get(0), r4.Get(1), r4.Get(2), t4);
MessageInterface::ShowMessage(" Ground station position in FK5: r3 = (%f, %f, %f)km at epoch = %18.12lf\n", r3.Get(0), r3.Get(1), r3.Get(2), t3);
MessageInterface::ShowMessage(" Transformation matrix from Earth fixed coordinate system to FK5 coordinate system at epoch = %18.12lf:\n", t3);
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(0,0), mt1(0,1), mt1(0,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(1,0), mt1(1,1), mt1(1,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(2,0), mt1(2,1), mt1(2,2));
#endif
Rvector3 uplinkVector = r4 - r3;
uplinkRange = uplinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range = r4-r3: %.12lf km\n",
uplinkRange);
#endif
// 4. Calculate up link range rate
Rvector3 p3V = uplinkLeg.GetVelocity(participants[0]);
Rvector3 p4V = uplinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("4. Get uplink leg range rate:\n");
MessageInterface::ShowMessage(" Ground station velocity in FK5: p3V = (%f, %f, %f)km/s\n", p3V.Get(0), p3V.Get(1), p3V.Get(2));
MessageInterface::ShowMessage(" Spacecraft velocity in FK5 : p4V = (%f, %f, %f)km/s\n", p4V.Get(0), p4V.Get(1), p4V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector upRRateVec = p4V - p3V /* - r12_j2k_vel*/ ;
rangeUnit = uplinkVector.GetUnitVector();
uplinkRangeRate = upRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range Rate: %.12lf km/s\n",
uplinkRangeRate);
#endif
// 4.1. Target range rate: Do we need this as well?
targetRangeRate = (downlinkRangeRate + uplinkRangeRate) / 2.0;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Target Range Rate: %.12lf km/s\n",
targetRangeRate);
#endif
// 5. Get sensors used in DSN 2-ways range
if (participantHardware.empty()||
((!participantHardware.empty())&&
participantHardware[0].empty()&&
participantHardware[1].empty()
)
)
{
// DO NOT LEAVE THIS RAW IN A SOURCE FILE!!!
// MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
// Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// Calculate real range
Real freqFactor = GetFrequencyFactor(frequency); // Notice that: unit of "frequency" varaibel is Hz (not MHz)
Real realTravelTime = uplinkTime + downlinkTime + receiveDelay + transmitDelay + targetDelay; // unit: second
Real realRangeKm = 0.5 *realTravelTime * GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/1000.0; // unit: km
Real realRange = realTravelTime * freqFactor; // unit: no unit
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Frequency = %.12lf MHz (This value is set to the default value in PhysicalMeasurement class due to no hardware used.)\n", frequency/1.0e6);
MessageInterface::ShowMessage(" Frequency factor = %.12lf MHz\n", freqFactor/1.0e6);
MessageInterface::ShowMessage(" Range in km = %.12lf km\n", realRangeKm);
MessageInterface::ShowMessage(" uplinkRange = %lfkm downlinkRange = %lfkm\n", uplinkRange, downlinkRange);
MessageInterface::ShowMessage(" receiveDelay = %lfm transmitDelay = %lfm targetDelay = %lfm\n", receiveDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM, transmitDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM, targetDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM);
MessageInterface::ShowMessage(" Range = %.12lf (It has no unit)\n", realRange);
#endif
// Set value for currentMeasurement
currentMeasurement.value[0] = realRange;
currentMeasurement.isFeasible = true;
return true;
}
ObjectArray objList1;
ObjectArray objList2;
ObjectArray objList3;
// objList1 := all transmitters in participantHardware list
// objList2 := all receivers in participantHardware list
// objList3 := all transponders in participantHardware list
for(std::vector<Hardware*>::iterator hw = this->participantHardware[0].begin();
hw != this->participantHardware[0].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transmitter")
objList1.push_back(*hw);
if ((*hw)->GetTypeName() == "Receiver")
objList2.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[1].begin();
hw != this->participantHardware[1].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transponder")
objList3.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
if (objList1.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 transmitter to send signal.\n");
throw new MeasurementException("The first participant does not have only 1 transmitter to send signal.\n");
}
if (objList2.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 receiver to receive signal.\n");
throw new MeasurementException("The first participant does not have only 1 receiver to receive signal.\n");
}
if (objList3.size() != 1)
{
MessageInterface::ShowMessage("The second participant does not have only 1 transponder to transpond signal.\n");
throw new MeasurementException("The second participant does not have only 1 transponder to transpond signal.\n");
}
Transmitter* gsTransmitter = (Transmitter*)objList1[0];
Receiver* gsReceiver = (Receiver*)objList2[0];
Transponder* scTransponder = (Transponder*)objList3[0];
if (gsTransmitter == NULL)
{
MessageInterface::ShowMessage("Transmitter is NULL object.\n");
throw new GmatBaseException("Transmitter is NULL object.\n");
}
if (gsReceiver == NULL)
{
MessageInterface::ShowMessage("Receiver is NULL object.\n");
throw new GmatBaseException("Receiver is NULL object.\n");
}
if (scTransponder == NULL)
{
MessageInterface::ShowMessage("Transponder is NULL object.\n");
throw new GmatBaseException("Transponder is NULL object.\n");
}
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("5. Sensors, delays, and signals:\n");
MessageInterface::ShowMessage(" List of sensors: %s, %s, %s\n",
gsTransmitter->GetName().c_str(), gsReceiver->GetName().c_str(),
scTransponder->GetName().c_str());
#endif
// 6. Get transmitter, receiver, and transponder delays:
transmitDelay = gsTransmitter->GetDelay();
receiveDelay = gsReceiver->GetDelay();
targetDelay = scTransponder->GetDelay();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Transmitter delay = %le s\n", gsTransmitter->GetDelay());
MessageInterface::ShowMessage(" Receiver delay = %le s\n", gsReceiver->GetDelay());
MessageInterface::ShowMessage(" Transponder delay = %le s\n", scTransponder->GetDelay());
#endif
// 7. Get frequency from transmitter of ground station (participants[0])
Signal* uplinkSignal = gsTransmitter->GetSignal();
Real uplinkFreq = uplinkSignal->GetValue();
// 8. Calculate media correction for uplink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("6. Media correction for uplink leg\n");
MessageInterface::ShowMessage(" UpLink signal frequency = %.12lf MHz\n", uplinkFreq);
#endif
Real roundTripTime = ((uplinkRange + downlinkRange)*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)/GmatTimeConstants::SECS_PER_DAY;
RealArray uplinkCorrection = CalculateMediaCorrection(uplinkFreq, r1, r2, currentMeasurement.epoch - roundTripTime);
Real uplinkRangeCorrection = uplinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real uplinkRealRange = uplinkRange + uplinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink range correction = %.12lf km\n",uplinkRangeCorrection);
MessageInterface::ShowMessage(" Uplink real range = %.12lf km\n",uplinkRealRange);
#endif
// 9. Doppler shift the frequency from the transmitter using uplinkRangeRate:
Real uplinkDSFreq = (1 - uplinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*uplinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("7. Transponder input and output frequencies\n");
MessageInterface::ShowMessage(" Uplink Doppler shift frequency = %.12lf MHz\n", uplinkDSFreq);
#endif
// 10.Set frequency for the input signal of transponder
Signal* inputSignal = scTransponder->GetSignal(0);
inputSignal->SetValue(uplinkDSFreq);
scTransponder->SetSignal(inputSignal, 0);
// 11. Check the transponder feasibility to receive the input signal:
if (scTransponder->IsFeasible(0) == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The transponder is unfeasible to receive uplink signal.\n");
throw new GmatBaseException("The transponder is unfeasible to receive uplink signal.\n");
}
// 12. Get frequency of transponder output signal
Signal* outputSignal = scTransponder->GetSignal(1);
Real downlinkFreq = outputSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink frequency = %.12lf Mhz\n", downlinkFreq);
#endif
// 13. Doppler shift the transponder output frequency by the downlinkRangeRate:
Real downlinkDSFreq = (1 - downlinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*downlinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Doppler shift frequency = %.12lf MHz\n", downlinkDSFreq);
#endif
// 14. Set frequency on receiver
Signal* downlinkSignal = gsReceiver->GetSignal();
downlinkSignal->SetValue(downlinkDSFreq);
// 15. Check the receiver feasibility to receive the downlink signal
if (gsReceiver->IsFeasible() == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The receiver is unfeasible to receive downlink signal.\n");
throw new MeasurementException("The receiver is unfeasible to receive downlink signal.\n");
}
// 16. Calculate media correction for downlink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("8. Media correction for downlink leg\n");
#endif
RealArray downlinkCorrection = CalculateMediaCorrection(downlinkDSFreq, r3, r4, currentMeasurement.epoch);
Real downlinkRangeCorrection = downlinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real downlinkRealRange = downlinkRange + downlinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink range correction = %.12lf km\n",downlinkRangeCorrection);
MessageInterface::ShowMessage(" Downlink real range = %.12lf km\n",downlinkRealRange);
#endif
// 17. Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("9. Travel time:\n");
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// 18. Calculate real range
// Real realRange = ((upRange + downRange) /
// (GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M) +
// receiveDelay + transmitDelay + targetDelay) * freqFactor;
// Real freqFactor = GetFrequencyFactor(frequency);
Real freqFactor = GetFrequencyFactor(uplinkFreq*1.0e6); // Notice that: unit of "uplinkFreq" is MHz (not Hz)
Real realTravelTime = uplinkTime + downlinkTime + receiveDelay + transmitDelay + targetDelay; // unit: second
Real realRangeKm = 0.5*realTravelTime * GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/1000.0; // unit: km
Real realRange = realTravelTime * freqFactor; // unit: no unit
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Frequency factor = %.12lf MHz\n", freqFactor/1.0e6);
MessageInterface::ShowMessage(" Calculated real range in km = %.12lf km\n", realRangeKm);
MessageInterface::ShowMessage(" Calculated real range = %.12lf (It has no unit)\n", realRange);
#endif
// 19. Set value for currentMeasurement
// currentMeasurement.value[0] = realRange;
currentMeasurement.value[0] = realRangeKm;
currentMeasurement.isFeasible = true;
retval = true;
}
return retval;
}
//-------------------------------------------------------------------------------
bool PolyhedronBody::LoadBodyShape()
{
if (isLoad)
return true;
// 1. Open data file:
std::ifstream bsFile(bodyShapeFilename.c_str());
if (!bsFile)
throw UtilityException("Error opening file: " +
bodyShapeFilename);
bsFile.setf(std::ios::skipws);
std::string line, subline;
// 2. Read all vertices and store them to pointsList:
Integer numvertices;
Integer index;
Real x, y, z;
Rvector3 vertex;
// 2.1. Read number of vertices:
getline(bsFile,line);
GmatStringUtil::ToInteger(line,&numvertices);
// 2.2. Read all vertices:
Integer first, last;
for(Integer i=0; (i < numvertices)&&(!bsFile.eof()); ++i)
{
// get a line from data file:
getline(bsFile,line);
// get record's index:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &index);
line = line.substr(first, line.size()-first);
// get vertex's x-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &x);
line = line.substr(first, line.size()-first);
// get vertex's y-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &y);
line = line.substr(first, line.size()-first);
// get vertex's z-coordinate:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToReal(subline, &z);
// push the vertex to verticesList:
vertex.Set(x, y,z); // unit: km
verticesList.push_back(vertex);
}
if (bsFile.eof())
throw UtilityException("Error: missing data in file: " +
bodyShapeFilename);
#ifdef DEBUG_READ_DATAFILE
MessageInterface::ShowMessage("number of vertices = %d\n", verticesList.size());
for (UnsignedInt i = 0; i < verticesList.size(); ++i)
MessageInterface::ShowMessage("%.15lf %.15lf %.15lf\n", verticesList[i].Get(0), verticesList[i].Get(1), verticesList[i].Get(2) );
MessageInterface::ShowMessage("\n");
#endif
// 3. Read all faces and store them to facesList:
Integer numfaces;
Integer ix, iy, iz;
PolygonFace triangularface;
// 3.1. Read number of faces:
getline(bsFile,line);
GmatStringUtil::ToInteger(line,&numfaces);
// 3.2. Read all faces:
for(Integer i=0; (i < numfaces)&&(!bsFile.eof()); ++i)
{
// get a line from data file:
getline(bsFile,line);
// get record's index:
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &index);
line = line.substr(first, line.size()-first);
// get the index of the first vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &ix);
line = line.substr(first, line.size()-first);
// get the index of the second vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &iy);
line = line.substr(first, line.size()-first);
// get the index of the third vertex of triangle :
line = GmatStringUtil::Trim(line, LEADING);
GmatStringUtil::FindFirstAndLast(line,' ',first,last);
subline = line.substr(0,first);
GmatStringUtil::ToInteger(subline, &iz);
// define a triangular face:
triangularface.push_back(ix-1); // GMAT index starts from 0; MatLab index starts from 1
triangularface.push_back(iy-1); // GMAT index starts from 0; MatLab index starts from 1
triangularface.push_back(iz-1); // GMAT index starts from 0; MatLab index starts from 1
// add the triangular face into facesList:
facesList.push_back(triangularface);
triangularface.clear();
}
if (bsFile.eof())
throw UtilityException("Error: missing data in file: " +
bodyShapeFilename);
// 4. Close data file:
if (bsFile.is_open()) bsFile.close();
isLoad = true;
#ifdef DEBUG_READ_DATAFILE
MessageInterface::ShowMessage("number of faces = %d\n", facesList.size());
for (UnsignedInt i = 0; i < facesList.size(); ++i)
MessageInterface::ShowMessage("%d: %d %d %d\n", i, facesList[i].at(0), facesList[i].at(1), facesList[i].at(2));
MessageInterface::ShowMessage("\n");
#endif
return true;
}
