//--------------------------------------------------------------------------- const Rvector6 Barycenter::GetMJ2000State(const A1Mjd &atTime) { // if it's built-in, get the state from the SpacePoint if (isBuiltIn) { lastStateTime = atTime; lastState = builtInSP->GetMJ2000State(atTime); #ifdef DEBUG_BARYCENTER_STATE MessageInterface::ShowMessage("Computing state for Barycenter %s, whose builtInSP is %s\n", instanceName.c_str(), (builtInSP->GetName()).c_str()); #endif return lastState; } // otherwise, sum the masses and states CheckBodies(); #ifdef DEBUG_BARYCENTER MessageInterface::ShowMessage("Entering BaryCenter::GetMJ2000EqState at time %12.10f\n", atTime.Get()); #endif Real bodyMass = 0.0; Rvector3 bodyPos(0.0,0.0,0.0); Rvector3 bodyVel(0.0,0.0,0.0); Real weight = 0.0; Rvector3 sumMassPos(0.0,0.0,0.0); Rvector3 sumMassVel(0.0,0.0,0.0); Rvector6 bodyState; Real sumMass = GetMass(); for (unsigned int i = 0; i < bodyList.size() ; i++) { bodyMass = ((CelestialBody*) (bodyList.at(i)))->GetMass(); bodyState = (bodyList.at(i))->GetMJ2000State(atTime); bodyPos = bodyState.GetR(); bodyVel = bodyState.GetV(); weight = bodyMass/sumMass; #ifdef DEBUG_BARYCENTER MessageInterface::ShowMessage("Mass (and weight) of body %s = %12.10f (%12.10f)\n", ((bodyList.at(i))->GetName()).c_str(), bodyMass, weight); MessageInterface::ShowMessage(" pos = %s\n", (bodyPos.ToString()).c_str()); MessageInterface::ShowMessage(" vel = %s\n", (bodyVel.ToString()).c_str()); #endif sumMassPos += (weight * bodyPos); sumMassVel += (weight * bodyVel); } #ifdef DEBUG_BARYCENTER MessageInterface::ShowMessage("sumMassPos = %s\n", (sumMassPos.ToString()).c_str()); #endif lastState.Set(sumMassPos(0), sumMassPos(1), sumMassPos(2), sumMassVel(0), sumMassVel(1), sumMassVel(2)); lastStateTime = atTime; return lastState; }
//------------------------------------------------------------------------------ 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 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; } }
//------------------------------------------------------------------------------ const Rvector3 SpaceObject::GetMJ2000Velocity(const A1Mjd &atTime) { const Rvector6 rv6 = GetMJ2000State(atTime); return (rv6.GetV()); }
//------------------------------------------------------------------------------ // static Rvector6 CartesianToKeplerian(const Rvector6 &cartVec, Real grav, // Real *ma); //------------------------------------------------------------------------------ Rvector6 CoordUtil::CartesianToKeplerian(const Rvector6 &cartVec, Real grav, Real *ma) { #ifdef DEBUG_CART_TO_KEPL MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian called ... \n")); #endif Real kepl[6]; Real r[3]; Real v[3]; Real tfp; Integer ret; Integer errorCode; for (int i=0; i<6; i++) kepl[i] = 0.0; if(grav < 1.0) { throw UtilityException(wxT("CoordUtil::CartesianToKeplerian() gravity constant ") wxT("too small for conversion to Keplerian elements\n")); } else { cartVec.GetR(r); cartVec.GetV(v); //MessageInterface::ShowMessage(wxT("CoordUtil::CartesianToKeplerian() r=%f, %f, %f ") // wxT("v=%f, %f, %f\n"), r[0], r[1], r[2], v[0], v[1], v[2]); if (CoordUtil::IsRvValid(r,v)) { errorCode = CoordUtil::ComputeCartToKepl(grav, r, v, &tfp, kepl, ma); switch (errorCode) { case 0: // no error ret = 1; break; case 2: throw UtilityException (wxT("CoordUtil::CartesianToKeplerian() ") wxT("Gravity constant too small for conversion to Keplerian elements\n")); default: throw UtilityException (wxT("CoordUtil::CartesianToKeplerian() ") wxT("Unable to convert Cartesian elements to Keplerian\n")); } } else { wxString ss; //ss << cartVec; throw UtilityException (wxT("CoordUtil::CartesianToKeplerian() Invalid Cartesian elements:\n") + ss); } } Rvector6 keplVec = Rvector6(kepl[0], kepl[1], kepl[2], kepl[3], kepl[4], kepl[5]); return keplVec; }
//--------------------------------------------------------------------------- const Rvector3 Barycenter::GetMJ2000Velocity(const A1Mjd &atTime) { Rvector6 tmp = GetMJ2000State(atTime); return (tmp.GetV()); }
//------------------------------------------------------------------------------ const Rvector3 BodyFixedPoint::GetMJ2000Velocity(const A1Mjd &atTime) { Rvector6 rv = GetMJ2000State(atTime); j2000Vel = rv.GetV(); return j2000Vel; }