//------------------------------------------------------------------------------
void ObjectReferencedAxes::SetZAxis(const std::string &toValue)
{
if ((toValue != "R") && (toValue != "V") && (toValue != "N") &&
(toValue != "-R") && (toValue != "-V") && (toValue != "-N") &&
(toValue != "r") && (toValue != "v") && (toValue != "n") &&
(toValue != "-r") && (toValue != "-v") && (toValue != "-n") && (toValue != "") )
throw CoordinateSystemException(
"ObjectReferencedAxes - Invalid value for Z-Axis");
zAxis = toValue;
}
//------------------------------------------------------------------------------
bool CoordinateConverter::ConvertFromBaseToBase(const A1Mjd &epoch,
SolarSystem *solarSystem,
const std::string &inBase,
const std::string &outBase,
const Real *inBaseState,
Real *outBaseState)
{
// if the base types are the same, just set the output state to the input state
if (inBase == outBase)
{
for (Integer i=0;i<6;i++) outBaseState[i] = inBaseState[i];
return true;
}
// Get rotation and rotation dot matrixes:
RotationMatrixFromICRFToFK5(epoch);
const Real* iToF = icrfToFK5.GetDataVector();
// calculate outBaseState:
if ((inBase == "ICRF") && (outBase == "FK5"))
{
outBaseState[0] = iToF[0]*inBaseState[0] + iToF[1]*inBaseState[1] + iToF[2]*inBaseState[2];
outBaseState[1] = iToF[3]*inBaseState[0] + iToF[4]*inBaseState[1] + iToF[5]*inBaseState[2];
outBaseState[2] = iToF[6]*inBaseState[0] + iToF[7]*inBaseState[1] + iToF[8]*inBaseState[2];
outBaseState[3] = iToF[0]*inBaseState[3] + iToF[1]*inBaseState[4] + iToF[2]*inBaseState[5];
outBaseState[4] = iToF[3]*inBaseState[3] + iToF[4]*inBaseState[4] + iToF[5]*inBaseState[5];
outBaseState[5] = iToF[6]*inBaseState[3] + iToF[7]*inBaseState[4] + iToF[8]*inBaseState[5];
}
else if ((inBase == "FK5") && (outBase == "ICRF"))
{
// Transpose the conversion matrix and multiply
outBaseState[0] = iToF[0]*inBaseState[0] + iToF[3]*inBaseState[1] + iToF[6]*inBaseState[2];
outBaseState[1] = iToF[1]*inBaseState[0] + iToF[4]*inBaseState[1] + iToF[7]*inBaseState[2];
outBaseState[2] = iToF[2]*inBaseState[0] + iToF[5]*inBaseState[1] + iToF[8]*inBaseState[2];
outBaseState[3] = iToF[0]*inBaseState[3] + iToF[3]*inBaseState[4] + iToF[6]*inBaseState[5];
outBaseState[4] = iToF[1]*inBaseState[3] + iToF[4]*inBaseState[4] + iToF[7]*inBaseState[5];
outBaseState[5] = iToF[2]*inBaseState[3] + iToF[5]*inBaseState[4] + iToF[8]*inBaseState[5];
}
else
{
std::string errmsg = "Cannot convert from coordinate system base type ";
errmsg += inBase + " to base type ";
errmsg += outBase + " - unknown base type.\n";
throw CoordinateSystemException(errmsg);
}
return true;
}
//------------------------------------------------------------------------------
bool ObjectReferencedAxes::SetStringParameter(const Integer id,
const std::string &value)
{
#ifdef DEBUG_ORA_SET
MessageInterface::ShowMessage
("ObjectReferencedAxes::SetStringParameter() entered, id=%d, value='%s'\n",
id, value.c_str());
#endif
bool OK = false;
if (!allowModify)
{
std::string errmsg = "Modifications to built-in coordinate system ";
errmsg += instanceName + " are not allowed.\n";
throw CoordinateSystemException(errmsg);
}
if ((UsesXAxis() != GmatCoordinate::NOT_USED) && (id == X_AXIS))
{
xAxis = value;
OK = true;
}
if ((UsesYAxis() != GmatCoordinate::NOT_USED) && (id == Y_AXIS))
{
yAxis = value;
OK = true;
}
if ((UsesZAxis() != GmatCoordinate::NOT_USED) && (id == Z_AXIS))
{
zAxis = value;
OK = true;
}
if ((UsesPrimary() != GmatCoordinate::NOT_USED) && (id == PRIMARY_OBJECT_NAME))
{
primaryName = value;
OK = true;
}
if ((UsesSecondary() != GmatCoordinate::NOT_USED) && (id == SECONDARY_OBJECT_NAME))
{
secondaryName = value;
OK = true;
}
if (OK) return true;
return DynamicAxes::SetStringParameter(id, value);
}
//------------------------------------------------------------------------------
bool CoordinateConverter::ConvertFromBaseToBase(const A1Mjd &epoch,
SolarSystem *solarSystem,
const std::string &inBase,
const std::string &outBase,
const Rvector &inBaseState,
Rvector &outBaseState)
{
// if the base types are the same, just set the output state to the input state
if (inBase == outBase)
{
if (inBaseState.GetSize() != outBaseState.GetSize())
{
std::string errmsg = "Cannot convert from coordinate system base type ";
errmsg += inBase + " to base type ";
errmsg += outBase + " - state vectors passed in are of differing sizes.\n";
throw CoordinateSystemException(errmsg);
}
for (Integer ii = 0; ii < inBaseState.GetSize(); ii++)
{
outBaseState[ii] = inBaseState[ii];
}
return true;
}
const Real *in = inBaseState.GetDataVector();
Real *out = new Real[outBaseState.GetSize()];
if (ConvertFromBaseToBase(epoch, solarSystem, inBase, outBase, in, out))
{
outBaseState.Set(out);
delete [] out;
return true;
}
delete [] out;
return false;
}
//------------------------------------------------------------------------------
bool CoordinateConverter::Convert(const A1Mjd &epoch, const Real *inState,
CoordinateSystem *inCoord, Real *outState,
CoordinateSystem *outCoord,
bool forceComputation, bool omitTranslation)
{
if ((!inCoord) || (!outCoord))
throw CoordinateSystemException(
"Undefined coordinate system - conversion not performed.");
#ifdef DEBUG_TO_FROM
MessageInterface::ShowMessage
("In Convert, inCoord is %s(%p) and outCoord is %s(%p)\n",
(inCoord->GetName()).c_str(), inCoord, (outCoord->GetName()).c_str(),
outCoord);
MessageInterface::ShowMessage
(" forceComputation=%d, omitTranslation=%d\n", forceComputation,
omitTranslation);
#endif
if (inCoord->GetName() == outCoord->GetName())
{
// assuming state is size 6 here!!!
for (Integer i=0;i<6;i++) outState[i] = inState[i];
lastRotMatrix.Set(1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0);
return true;
}
#ifdef DEBUG_FIRST_CALL
if ((firstCallFired == false) || (epoch.Get() == GmatTimeConstants::MJD_OF_J2000))
{
MessageInterface::ShowMessage(
"Coordinate conversion check:\n %s --> %s\n",
inCoord->GetName().c_str(), outCoord->GetName().c_str());
MessageInterface::ShowMessage(
" Epoch: %.12lf\n", epoch.Get());
MessageInterface::ShowMessage(
" input State = [%.10lf %.10lf %.10lf %.16lf %.16lf %.16lf]\n",
inState[0], inState[1], inState[2], inState[3], inState[4],
inState[5]);
}
#endif
#ifdef DEBUG_TO_FROM
MessageInterface::ShowMessage
(" inCoord->GetOrigin=%s(%p), outCoord->GetOrigin=%s(%p)\n",
inCoord->GetOrigin()->GetName().c_str(), inCoord->GetOrigin(),
outCoord->GetOrigin()->GetName().c_str(), outCoord->GetOrigin());
#endif
// call coordinate system methods to convert - allow exceptions to
// percolate up (to be caught at a higher level)
bool sameOrigin = (inCoord->GetOrigin() == outCoord->GetOrigin() ?
true : false);
bool coincident = sameOrigin || omitTranslation;
Real intState[6];
#ifdef DEBUG_TO_FROM
MessageInterface::ShowMessage
(" sameOrigin=%d, omitTranslation=%d, coincident=%d\n",
sameOrigin, omitTranslation, coincident);
#endif
inCoord->ToBaseSystem(epoch, inState, intState, coincident, forceComputation);
#ifdef DEBUG_TO_FROM
MessageInterface::ShowMessage
("In Convert, sameOrigin is %s,and coincident is %s\n",
(sameOrigin? "TRUE" : "FALSE"),
(coincident? "TRUE" : "FALSE"));
MessageInterface::ShowMessage("inState = %18.10f %18.10f %18.10f\n",
inState[0], inState[1], inState[2]);
MessageInterface::ShowMessage(" %18.10f %18.10f %18.10f\n",
inState[3], inState[4], inState[5]);
MessageInterface::ShowMessage("intState = %18.10f %18.10f %18.10f\n",
intState[0], intState[1], intState[2]);
MessageInterface::ShowMessage(" %18.10f %18.10f %18.10f\n",
intState[3], intState[4], intState[5]);
#endif
std::string inBaseName = inCoord->GetBaseSystem();
std::string outBaseName = outCoord->GetBaseSystem();
#ifdef DEBUG_BASE_SYSTEM
MessageInterface::ShowMessage("inBase system = %s\n", inBaseName.c_str());
MessageInterface::ShowMessage("outBase system = %s\n", outBaseName.c_str());
#endif
Real baseState[6];
if (inBaseName != outBaseName)
{
ConvertFromBaseToBase(epoch, inCoord->GetSolarSystem(), inBaseName, outBaseName, intState, baseState);
}
else
{
for (unsigned int ii = 0; ii < 6; ii++)
baseState[ii] = intState[ii];
}
#ifdef DEBUG_BASE_SYSTEM
MessageInterface::ShowMessage("baseState = %18.10f %18.10f %18.10f\n",
baseState[0], baseState[1], baseState[2]);
MessageInterface::ShowMessage(" %18.10f %18.10f %18.10f\n",
baseState[3], baseState[4], baseState[5]);
MessageInterface::ShowMessage(" ... about to call outCoord->FromBaseSystem ... outCoord is %sNULL\n",
(outCoord? "NOT " : ""));
MessageInterface::ShowMessage("epoch = %le, coincident = %s, forceComputation = %s\n", epoch.Get(),
(coincident? "true" : "false"), (forceComputation? "true" : "false"));
#endif
outCoord->FromBaseSystem(epoch, baseState, outState, coincident, forceComputation);
#ifdef DEBUG_TO_FROM
MessageInterface::ShowMessage("outState = %18.10f %18.10f %18.10f\n",
outState[0], outState[1], outState[2]);
MessageInterface::ShowMessage(" %18.10f %18.10f %18.10f\n",
outState[3], outState[4], outState[5]);
#endif
// last rotation matrix
// R1 * R2T
Real R1[9];
Real R2[9];
Real lrm[3][3];
Integer p3;
inCoord->GetLastRotationMatrix(R1);
outCoord->GetLastRotationMatrix(R2);
Real R2T[9] = {R2[0], R2[3], R2[6],
R2[1], R2[4], R2[7],
R2[2], R2[5], R2[8]};
for (Integer p = 0; p < 3; ++p)
{
p3 = 3*p;
for (Integer q = 0; q < 3; ++q)
{
lrm[p][q] = R2T[p3] * R1[q] +
R2T[p3+1] * R1[q+3] +
R2T[p3+2] * R1[q+6];
}
}
lastRotMatrix.Set(lrm[0][0],lrm[0][1],lrm[0][2],
lrm[1][0],lrm[1][1],lrm[1][2],
lrm[2][0],lrm[2][1],lrm[2][2]);
// last rotation Dot matrix
// R1*R2Dot + R1dot*R2
Real R1dot[9];
Real R2dot[9];
inCoord->GetLastRotationDotMatrix(R1dot);
outCoord->GetLastRotationDotMatrix(R2dot);
#ifdef DEBUG_ROT_DOT_MATRIX
MessageInterface::ShowMessage("R1dot = %le %le %le %le %le %le %le %le %le\n",
R1dot[0],R1dot[1],R1dot[2],R1dot[3],R1dot[4],R1dot[5],R1dot[6],R1dot[7],R1dot[8]);
MessageInterface::ShowMessage("R2dot = %le %le %le %le %le %le %le %le %le\n",
R2dot[0],R2dot[1],R2dot[2],R2dot[3],R2dot[4],R2dot[5],
R2dot[6],R2dot[7],R2dot[8]);
#endif
Real R2dotTR1[3][3];
Real R2TR1dot[3][3];
Real R2dotT[9] = {R2dot[0], R2dot[3], R2dot[6],
R2dot[1], R2dot[4], R2dot[7],
R2dot[2], R2dot[5], R2dot[8]};
for (Integer p = 0; p < 3; ++p)
{
p3 = 3*p;
for (Integer q = 0; q < 3; ++q)
{
R2dotTR1[p][q] = R2dotT[p3] * R1[q] +
R2dotT[p3+1] * R1[q+3] +
R2dotT[p3+2] * R1[q+6];
}
}
for (Integer p = 0; p < 3; ++p)
{
p3 = 3*p;
for (Integer q = 0; q < 3; ++q)
{
R2TR1dot[p][q] = R2T[p3] * R1dot[q] +
R2T[p3+1] * R1dot[q+3] +
R2T[p3+2] * R1dot[q+6];
}
}
lastRotDotMatrix.Set(R2dotTR1[0][0] + R2TR1dot[0][0], R2dotTR1[0][1] + R2TR1dot[0][1], R2dotTR1[0][2] + R2TR1dot[0][2],
R2dotTR1[1][0] + R2TR1dot[1][0], R2dotTR1[1][1] + R2TR1dot[1][1], R2dotTR1[1][2] + R2TR1dot[1][2],
R2dotTR1[2][0] + R2TR1dot[2][0], R2dotTR1[2][1] + R2TR1dot[2][1], R2dotTR1[2][2] + R2TR1dot[2][2]);
#ifdef DEBUG_FIRST_CALL
if ((firstCallFired == false) || (epoch.Get() == GmatTimeConstants::MJD_OF_J2000))
{
MessageInterface::ShowMessage(
" internal State = [%.10lf %.10lf %.10lf %.16lf %.16lf %.16lf]\n",
intState[0], intState[1], intState[2], intState[3],
intState[4], intState[5]);
MessageInterface::ShowMessage(
" output State = [%.10lf %.10lf %.10lf %.16lf %.16lf %.16lf]\n",
outState[0], outState[1], outState[2], outState[3], outState[4],
outState[5]);
firstCallFired = true;
}
#endif
return true;
}
//---------------------------------------------------------------------------
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;
}
}
