//------------------------------------------------------------------------------ // void ShowDerivative(const wxString &header, Real *state, Integer satCount) //------------------------------------------------------------------------------ void PointMassForce::ShowDerivative(const wxString &header, Real *state, Integer satCount) { #if DEBUG_PMF_DERV static int debugCount2 = 0; static bool showDeriv = true; if (showDeriv) { MessageInterface::ShowMessage(wxT("%s\n"), header.c_str()); MessageInterface::ShowMessage( wxT(">>>>>=======================================\n")); Integer i6; Rvector6 stateVec = Rvector6(state); for (Integer i = 0; i < satCount; i++) { i6 = cartIndex + i * 6; MessageInterface::ShowMessage (wxT("sc#=%d state=%s\n"), i, stateVec.ToString().c_str()); MessageInterface::ShowMessage (wxT("deriv=%f %f %f %f %f %f\n"), deriv[i6], deriv[i6+1], deriv[i6+2], deriv[i6+3], deriv[i6+4], deriv[i6+5]); } MessageInterface::ShowMessage( wxT("=======================================<<<<<\n")); debugCount2++; if (debugCount2 > 10) showDeriv = false; } #endif }
Rvector6 SpaceObject::GetLastState() { Rvector6 result; Real *st = state.GetState(); result.Set(st[0], st[1], st[2], st[3], st[4], st[5]); return result; }
//--------------------------------------------------------------------------- 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; }
//------------------------------------------------------------------------------ Real EnvData::GetEnvReal(const std::string &str) { //------------------------------------------------------- // 1. Get Spacecraft's central body (It's done in InitializeRefObjects()) // 2. Get current time (use parameter or sc pointer?) // 3. Get spacecraft's position (use parameter?) // 4. Call GetDensity() on central body //------------------------------------------------------- if (mSpacecraft == NULL || mSolarSystem == NULL || mOrigin == NULL) InitializeRefObjects(); if (str == "AtmosDensity") { Real a1mjd = mSpacecraft->GetRealParameter("A1Epoch"); Rvector6 cartState = mSpacecraft->GetState().GetState(); // // Rvector6 cartState = mSpacecraft->GetStateVector("Cartesian"); // Rvector6 cartState = mSpacecraft->GetState(0); // Real state[6]; // for (int i=0; i<6; i++) // state[i] = cartState[i]; Real *state = (Real*)cartState.GetDataVector(); Real density = 0.0; // Call GetDensity() on if origin is CelestialBody if (mOrigin->IsOfType(Gmat::CELESTIAL_BODY)) { if (((CelestialBody*)mOrigin)->GetDensity(state, &density, a1mjd, 1)) { #ifdef DEBUG_ENVDATA_RUN MessageInterface::ShowMessage ("EnvData::GetEnvReal() mOrigin=%s, a1mjd=%f, state=%s, density=%g\n", mOrigin->GetName().c_str(), a1mjd, cartState.ToString().c_str(), density); #endif } else { #ifdef DEBUG_ENVDATA_RUN MessageInterface::ShowMessage ("EnvData::GetEnvReal() AtmosphereModel used for %s is NULL\n", mOrigin->GetName().c_str()); #endif } } return density; } else { throw ParameterException("EnvData::GetEnvReal() Unknown parameter name: " + str); } }
//------------------------------------------------------------------------------ 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); }
int main(int argc, char **argv) { std::cout << "************************************************\n" << "*** StateType Conversion Unit Test Program\n" << "************************************************\n\n"; StateConverter stateConverter; Rvector6 newState; Real state[6]; state[0] = 7100; state[1] = 0.0; state[2] = 1300; state[3] = 0.0; state[4] = 7.35; state[5] = 1.0; std::cout << "\n--- Beginning with state ----"; std::cout << std::setprecision(8); for (int i=0; i < 6; i++) std::cout << "\n[" << i << "]: " << state[i]; std::cout << "\n--- Converting to Keplerian state ----"; newState = stateConverter.Convert(state,"Cartesian","Keplerian"); for (int i=0; i < 6; i++) { state[i] = newState.Get(i); std::cout << "\n[" << i << "]: " << state[i]; } std::cout << "\n--- Converting to Cartesian state ----"; newState = stateConverter.Convert(state,"Keplerian","Cartesian"); for (int i=0; i < 6; i++) { state[i] = newState.Get(i); std::cout << "\n[" << i << "]: " << state[i]; } std::cout << "\n********************* End of Testing ********************\n"; }
//------------------------------------------------------------------------------ const Rvector6 BodyFixedPoint::GetMJ2000State(const A1Mjd &atTime) { #ifdef DEBUG_BODYFIXED_STATE MessageInterface::ShowMessage("In GetMJ2000State for BodyFixedPoint %s\n", instanceName.c_str()); #endif UpdateBodyFixedLocation(); Real epoch = atTime.Get(); Rvector6 bfState; // For now I'm ignoring velocity; this assumes bfLocation is kept up-to-date bfState.Set(bfLocation[0], bfLocation[1], bfLocation[2], 0.0, 0.0, 0.0); // Convert from the body-fixed location to a J2000 location, // assuming you have pointer to coordinate systems mj2k and bfcs, // where mj2k is a J2000 system and bfcs is BodyFixed #ifdef DEBUG_BODYFIXED_STATE MessageInterface::ShowMessage("... before call to Convert, epoch = %12.10f\n", epoch); MessageInterface::ShowMessage(" ... bfcs (%s) = %s and mj2kcs (%s) = %s\n", bfcsName.c_str(), (bfcs? "NOT NULL" : "NULL"), mj2kcsName.c_str(), (mj2kcs? "NOT NULL" : "NULL")); MessageInterface::ShowMessage("bf state (in bfcs, cartesian) = %s\n", (bfState.ToString()).c_str()); MessageInterface::ShowMessage("SolarSystem is%s NULL\n", (solarSystem? "NOT " : "")); #endif ccvtr.Convert(epoch, bfState, bfcs, j2000PosVel, mj2kcs); #ifdef DEBUG_BODYFIXED_STATE MessageInterface::ShowMessage("bf state (in mj2kcs, cartesian) = %s\n", (j2000PosVel.ToString()).c_str()); #endif lastStateTime = atTime; lastState = j2000PosVel; return j2000PosVel; }
//------------------------------------------------------------------------------ // void ShowBodyState(const wxString &header, Real time, Rvector6 &rv); //------------------------------------------------------------------------------ void PointMassForce::ShowBodyState(const wxString &header, Real time, Rvector6 &rv) { #if DEBUG_PMF_BODY static int debugCount1 = 0; static bool showBodyState = true; if (showBodyState) { MessageInterface::ShowMessage(wxT("%s\n"), header.c_str()); MessageInterface::ShowMessage( wxT(">>>>>=======================================\n")); MessageInterface::ShowMessage(wxT("time=%f rv=%s\n"), time, rv.ToString().c_str()); MessageInterface::ShowMessage( wxT("=======================================<<<<<\n")); debugCount1++; if (debugCount1 > 10) showBodyState = false; } #endif }
void printState(const std::string &title, const Rvector6 state) { std::cout << "\n--------- " << title << " ---------\n"; for (int i=0; i < 6; i++) std::cout << "[" << i << "]: " << state.Get(i) << std::endl; }
//------------------------------------------------------------------------------ const Rvector3 SpaceObject::GetMJ2000Velocity(const A1Mjd &atTime) { const Rvector6 rv6 = GetMJ2000State(atTime); return (rv6.GetV()); }
//------------------------------------------------------------------------------ const Rvector3 SpaceObject::GetMJ2000Position(const A1Mjd &atTime) { const Rvector6 rv6 = GetMJ2000State(atTime); return (rv6.GetR()); }
//------------------------------------------------------------------------------ // 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; }
//------------------------------------------------------------------------------ void GravityField::Calculate (Real dt, Real state[6], Real acc[3], Rmatrix33& grad) { #ifdef DEBUG_CALCULATE MessageInterface::ShowMessage( "Entering Calculate with dt = %12.10f, state = %12.10f %12.10f %12.10f %12.10f %12.10f %12.10f\n", dt, state[0], state[1], state[2], state[3], state[4], state[5]); MessageInterface::ShowMessage(" acc = %12.10f %12.10f %12.10f\n", acc[0], acc[1], acc[2]); #endif Real jday = epoch + GmatTimeConstants::JD_JAN_5_1941 + dt/GmatTimeConstants::SECS_PER_DAY; // convert to body fixed coordinate system Real now = epoch + dt/GmatTimeConstants::SECS_PER_DAY; Real tmpState[6]; // CoordinateConverter cc; - move back to class, for performance cc.Convert(now, state, inputCS, tmpState, fixedCS); // which CSs to use here??? #ifdef DEBUG_CALCULATE MessageInterface::ShowMessage( "After Convert, jday = %12.10f, now = %12.10f, and tmpState = %12.10f %12.10f %12.10f %12.10f %12.10f %12.10f\n", jday, now, tmpState[0], tmpState[1], tmpState[2], tmpState[3], tmpState[4], tmpState[5]); #endif Rmatrix33 rotMatrix = cc.GetLastRotationMatrix(); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("---->>>> rotMatrix = %s\n", rotMatrix.ToString().c_str()); #endif // calculate sun and moon pos Real sunpos[3] = {0.0,0.0,0.0}; Real moonpos[3] = {0.0,0.0,0.0}; Real sunMass = 0.0; Real moonMass = 0.0; // Acceleration Real rotacc[3]; Rmatrix33 rotgrad; bool useTides; // for now, "None" and "SolidAndPole" are the only valid EarthTideModel values if ((bodyName == GmatSolarSystemDefaults::EARTH_NAME) && (GmatStringUtil::ToUpper(earthTideModel) == "SOLIDANDPOLE")) { Real ep = epoch + dt / GmatTimeConstants::SECS_PER_DAY; // isn't this the same as now? CelestialBody* theSun = solarSystem->GetBody(SolarSystem::SUN_NAME); CelestialBody* theMoon = solarSystem->GetBody(SolarSystem::MOON_NAME); if (!theSun || !theMoon) throw ODEModelException("Solar system does not contain the Sun or Moon for Tide model."); Rvector6 sunstateinertial = theSun->GetState(ep); Rvector6 moonstateinertial = theMoon->GetState(ep); Rvector6 sunstate; Rvector6 moonstate; cc.Convert(now, sunstateinertial, inputCS, sunstate, fixedCS); cc.Convert(now, moonstateinertial, inputCS, moonstate, fixedCS); sunstate.GetR(sunpos); moonstate.GetR(moonpos); sunMass = theSun->GetMass(); moonMass = theMoon->GetMass(); useTides = true; } else useTides = false; #ifdef DEBUG_GRAVITY_EARTH_TIDE MessageInterface::ShowMessage("Calling gravityModel->CalculateFullField with useTides = %s\n", (useTides? "true" : "false")); #endif // Get xp and yp from the EOP file Real xp, yp, lod; Real utcmjd = TimeConverterUtil::Convert(now, TimeConverterUtil::A1MJD, TimeConverterUtil::UTCMJD, GmatTimeConstants::JD_JAN_5_1941); eop->GetPolarMotionAndLod(utcmjd, xp, yp, lod); bool computeMatrix = fillAMatrix || fillSTM; gravityModel->CalculateFullField(jday, tmpState, degree, order, useTides, sunpos, moonpos, sunMass, moonMass, xp, yp, computeMatrix, rotacc, rotgrad); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("after CalculateFullField, rotgrad = %s\n", rotgrad.ToString().c_str()); #endif /* MessageInterface::ShowMessage ("HarmonicField::Calculate pos= %20.14f %20.14f %20.14f\n", tmpState[0],tmpState[1],tmpState[2]); MessageInterface::ShowMessage ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n", rotgrad(0,0),rotgrad(0,1),rotgrad(0,2)); MessageInterface::ShowMessage ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n", rotgrad(1,0),rotgrad(1,1),rotgrad(1,2)); MessageInterface::ShowMessage ("HarmonicField::Calculate grad= %20.14e %20.14e %20.14e\n", rotgrad(2,0),rotgrad(2,1),rotgrad(2,2)); */ // Convert back to target CS InverseRotate (rotMatrix,rotacc,acc); grad = rotMatrix.Transpose() * rotgrad * rotMatrix; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("at end of Calculate, after rotation, grad = %s\n", grad.ToString().c_str()); #endif }
//--------------------------------------------------------------------------- const Rvector3 Barycenter::GetMJ2000Position(const A1Mjd &atTime) { Rvector6 tmp = GetMJ2000State(atTime); return (tmp.GetR()); }
//------------------------------------------------------------------------------ const Rvector3 BodyFixedPoint::GetMJ2000Velocity(const A1Mjd &atTime) { Rvector6 rv = GetMJ2000State(atTime); j2000Vel = rv.GetV(); return j2000Vel; }
//------------------------------------------------------------------------------ const Rvector3 BodyFixedPoint::GetMJ2000Position(const A1Mjd &atTime) { Rvector6 rv = GetMJ2000State(atTime); j2000Pos = rv.GetR(); return j2000Pos; }
//--------------------------------------------------------------------------- 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 Barycenter::GetMJ2000Velocity(const A1Mjd &atTime) { Rvector6 tmp = GetMJ2000State(atTime); return (tmp.GetV()); }
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)); */ }
//------------------------------------------------------------------------------ Rvector6 PointMassForce::GetDerivativesForSpacecraft(Spacecraft *sc) { Rvector6 dv; Real *j2kState = sc->GetState().GetState(); Real state[6]; Real now = sc->GetEpoch(); BuildModelState(now, state, j2kState); #ifdef DEBUG_FIRST_CALL if (!firstCallFired) MessageInterface::ShowMessage("Point Mass %s: Pos = [%lf %lf %lf] -> ", bodyName.c_str(), state[0], state[1], state[2]); #endif Real radius, r3, mu_r, rbb3, mu_rbb, a_indirect[3]; Real relativePosition[3]; Rvector6 bodyrv = body->GetState(now); Rvector6 orig = forceOrigin->GetMJ2000State(now); const Real *brv = bodyrv.GetDataVector(), *orv = orig.GetDataVector(); Real rv[3]; rv[0] = brv[0] - orv[0]; rv[1] = brv[1] - orv[1]; rv[2] = brv[2] - orv[2]; // The vector from the force origin to the gravitating body // Precalculations for the indirect effect term rbb3 = rv[0]*rv[0]+rv[1]*rv[1]+rv[2]*rv[2]; if (rbb3 != 0.0) { //rbb3 *= sqrt(rbb3); rbb3 = sqrt(rbb3 * rbb3 * rbb3); mu_rbb = mu / rbb3; a_indirect[0] = mu_rbb * rv[0]; a_indirect[1] = mu_rbb * rv[1]; a_indirect[2] = mu_rbb * rv[2]; } else a_indirect[0] = a_indirect[1] = a_indirect[2] = 0.0; { //relativePosition[0] = rv[0] - state[0]; //relativePosition[1] = rv[1] - state[1]; //relativePosition[2] = rv[2] - state[2]; relativePosition[0] = - state[0]; relativePosition[1] = - state[1]; relativePosition[2] = - state[2]; r3 = relativePosition[0]*relativePosition[0] + relativePosition[1]*relativePosition[1] + relativePosition[2]*relativePosition[2]; radius = sqrt(r3); r3 *= radius; mu_r = mu / r3; dv[3] = relativePosition[0] * mu_r - a_indirect[0]; dv[4] = relativePosition[1] * mu_r - a_indirect[1]; dv[5] = relativePosition[2] * mu_r - a_indirect[2]; #ifdef DEBUG_DERIVATIVES_FOR_SPACECRAFT MessageInterface::ShowMessage("***PMF AX in GD4SC: %.12lf * %.12le -" " %.12le = %.12le\n", relativePosition[0], mu_r, a_indirect[0], dv[3]); #endif } #ifdef DEBUG_FIRST_CALL if (!firstCallFired) MessageInterface::ShowMessage("Accel = [%le %le %le]\n", dv[3], dv[4], dv[5]); #endif return dv; }