//------------------------------------------------------------------------------
bool SolarPowerSystem::Initialize()
{
#ifdef DEBUG_SOLAR_POWER
MessageInterface::ShowMessage("Calling Initialization on %s\n",
instanceName.c_str());
MessageInterface::ShowMessage("number of shadow bodies = %d\n",
(Integer) shadowBodyNames.size());
#endif
PowerSystem::Initialize();
// Solar System is set by the spacecraft to which this is attached
if (!solarSystem)
{
std::string errmsg = "SolarSystem has not been set on PowerSystem ";
errmsg += instanceName + ".\n";
throw HardwareException(errmsg);
}
// if no names were added to the ShadowBodies list, add the Default body
// This will cause "ShadowBodies = {'Earth'} to be written to the script <<
if ((shadowBodyNames.empty()) && (!settingNoBodies))
shadowBodyNames = defaultShadowBodyNames;
// shadowBodyNames.push_back("Earth");
// Set up the list of shadowBodies using current solarSystem
shadowBodies.clear();
for (unsigned int ii = 0; ii < shadowBodyNames.size(); ii++)
{
CelestialBody *body = solarSystem->GetBody(shadowBodyNames.at(ii));
if (!body)
{
std::string errmsg = "SolarPowerSystem ";
errmsg += instanceName + " cannot find body ";
errmsg += shadowBodyNames.at(ii) + ". ShadowBodies must be ";
errmsg += "Celestial Bodies.\n";
throw HardwareException(errmsg);
}
shadowBodies.push_back(body);
#ifdef DEBUG_SOLAR_POWER
MessageInterface::ShowMessage("Adding shadow body %s to %s\n",
body->GetName().c_str(), instanceName.c_str());
#endif
}
if (!shadowState)
shadowState = new ShadowState();
shadowState->SetSolarSystem(solarSystem);
return isInitialized;
}
//------------------------------------------------------------------------------
bool GravityField::GetDerivatives(Real * state, Real dt, Integer dvorder,
const Integer id)
{
#ifdef DEBUG_FIRST_CALL
if (firstCallFired == false)
{
MessageInterface::ShowMessage(
"GravityField(%s) inputs:\n"
" state = [%.10lf %.10lf %.10lf %.16lf %.16lf %.16lf]\n"
" dt = %.10lf\n dvorder = %d\n",
instanceName.c_str(), state[0], state[1], state[2], state[3],
state[4], state[5], dt, dvorder);
}
#endif
// We may want to do this down the road:
// if (fabs(state[0]) + fabs(state[1]) + fabs(state[2]) < minimumDistance)
// throw ODEModelException("A harmonic gravity field is being computed "
// "inside of the " + bodyName + ", which is not allowed");
if ((dvorder > 2) || (dvorder < 1))
return false;
#ifdef DEBUG_GRAVITY_FIELD
MessageInterface::ShowMessage("%s %d %s %le %s %le %le %le %le %le %le\n",
"Entered GravityField::GetDerivatives with order", dvorder, "dt = ",
dt, "and state\n",
state[0], state[1], state[2], state[3], state[4], state[5]);
MessageInterface::ShowMessage("cartesianCount = %d, stmCount = %d, aMatrixCount = %d\n",
cartesianCount, stmCount, aMatrixCount);
MessageInterface::ShowMessage("fillCartesian = %s, fillSTM = %s, fillAMatrix = %s\n",
(fillCartesian? "true" : "false"), (fillSTM? "true" : "false"), (fillAMatrix? "true" : "false"));
MessageInterface::ShowMessage("cartesianStart = %d, stmStart = %d, aMatrixStart = %d\n",
cartesianStart, stmStart, aMatrixStart);
#endif
/// @todo Optimize this code -- May be possible to make GravityField calculations more efficient
if ((cartesianCount < 1) && (stmCount < 1) && (aMatrixCount < 1))
throw ODEModelException(
"GravityField requires at least one spacecraft.");
// todo: Move into header; this flag is used to decide if the velocity terms
// are copied into the position derivatives for first order integrators, so
// when the GravityField is set to work at non-central bodies, the detection
// will need to happen in initialization.
Real satState[6];
Integer nOffset;
now = epoch + dt/GmatTimeConstants::SECS_PER_DAY;
#ifdef DEBUG_GRAV_COORD_SYSTEM
MessageInterface::ShowMessage(
"------ body = %s\n------ inputCS = %s\n------ targetCS = %s"
"\n------ fixedCS = %s\n",
body->GetName().c_str(), (inputCS == NULL? "NULL" : inputCS->GetName().c_str()),
(targetCS == NULL? "NULL" : targetCS->GetName().c_str()), (fixedCS == NULL? "NULL" : fixedCS->GetName().c_str()));
#endif
#ifdef DEBUG_FIRST_CALL
if (firstCallFired == false)
{
CelestialBody *targetBody = (CelestialBody*) targetCS->GetOrigin();
CelestialBody *fixedBody = (CelestialBody*) fixedCS->GetOrigin();
MessageInterface::ShowMessage(
" Epoch = %.12lf\n targetBody = %s\n fixedBody = %s\n",
now.Get(), targetBody->GetName().c_str(),
fixedBody->GetName().c_str());
MessageInterface::ShowMessage(
"------ body = %s\n------ inputCS = %s\n------ targetCS = %s\n"
"------ fixedCS = %s\n",
body->GetName().c_str(), inputCS->GetName().c_str(),
targetCS->GetName().c_str(), fixedCS->GetName().c_str());
}
#endif
if (fillCartesian || fillAMatrix || fillSTM)
{
// See assumption 1, above
if ((cartesianCount < stmCount) || (cartesianCount < aMatrixCount))
{
throw ODEModelException("GetDerivatives: cartesianCount < stmCount or aMatrixCount\n");
}
Real originacc[3] = { 0.0,0.0,0.0 }; // JPD code
Rmatrix33 origingrad (0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
Rmatrix33 emptyGradient(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
Rmatrix33 gradnew (0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
if (body != forceOrigin)
{
Real originstate[6] = { 0.0,0.0,0.0,0.0,0.0,0.0 };
Calculate(dt,originstate,originacc,origingrad);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("---------> origingrad = %s\n", origingrad.ToString().c_str());
#endif
}
for (Integer n = 0; n < cartesianCount; ++n)
{
nOffset = cartesianStart + n * stateSize;
for (Integer i = 0; i < 6; ++i)
satState[i] = state[i+nOffset];
Real accnew[3]; // JPD code
gradnew = emptyGradient;
Calculate(dt,satState,accnew,gradnew);
if (body != forceOrigin)
{
for (Integer i=0; i<=2; ++i)
accnew[i] -= originacc[i];
gradnew -= origingrad;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("---------> body not equal to forceOrigin\n");
#endif
}
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("---------> gradnew (%d) = %s\n", n, gradnew.ToString().c_str());
#endif
// Fill Derivatives
switch (dvorder)
{
case 1:
deriv[0+nOffset] = satState[3];
deriv[1+nOffset] = satState[4];
deriv[2+nOffset] = satState[5];
deriv[3+nOffset] = accnew[0];
deriv[4+nOffset] = accnew[1];
deriv[5+nOffset] = accnew[2];
break;
case 2:
deriv[0+nOffset] = accnew[0];
deriv[1+nOffset] = accnew[1];
deriv[2+nOffset] = accnew[2];
deriv[3+nOffset] = 0.0;
deriv[4+nOffset] = 0.0;
deriv[5+nOffset] = 0.0;
break;
}
#ifdef DEBUG_DERIVATIVES
for (Integer ii = 0 + nOffset; ii < 6+nOffset; ii++)
MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", ii, deriv[ii]);
#endif
if (fillSTM)
{
Real aTilde[36];
Integer element;
// @todo Add the use of the GetAssociateIndex() method here to get index into state array
// (See assumption 1, above)
if (n <= stmCount)
{
Integer i6 = stmStart + n * 36;
// Calculate A-tilde
aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
aTilde[ 9] = aTilde[10] = aTilde[11] =
aTilde[12] = aTilde[13] = aTilde[14] =
aTilde[15] = aTilde[16] = aTilde[17] =
aTilde[21] = aTilde[22] = aTilde[23] =
aTilde[27] = aTilde[28] = aTilde[29] =
aTilde[33] = aTilde[34] = aTilde[35] = 0.0;
// fill in the lower left quadrant with the calculated gradient values
aTilde[18] = gradnew(0,0);
aTilde[19] = gradnew(0,1);
aTilde[20] = gradnew(0,2);
aTilde[24] = gradnew(1,0);
aTilde[25] = gradnew(1,1);
aTilde[26] = gradnew(1,2);
aTilde[30] = gradnew(2,0);
aTilde[31] = gradnew(2,1);
aTilde[32] = gradnew(2,2);
for (Integer j = 0; j < 6; j++)
{
for (Integer k = 0; k < 6; k++)
{
element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", (i6+element), aTilde[element]);
#endif
deriv[i6+element] = aTilde[element];
}
}
}
}
if (fillAMatrix)
{
Real aTilde[36];
Integer element;
// @todo Add the use of the GetAssociateIndex() method here to get index into state array
// (See assumption 1, above)
if (n <= aMatrixCount)
{
Integer i6 = aMatrixStart + n * 36;
// Calculate A-tilde
aTilde[ 0] = aTilde[ 1] = aTilde[ 2] =
aTilde[ 3] = aTilde[ 4] = aTilde[ 5] =
aTilde[ 6] = aTilde[ 7] = aTilde[ 8] =
aTilde[ 9] = aTilde[10] = aTilde[11] =
aTilde[12] = aTilde[13] = aTilde[14] =
aTilde[15] = aTilde[16] = aTilde[17] =
aTilde[21] = aTilde[22] = aTilde[23] =
aTilde[27] = aTilde[28] = aTilde[29] =
aTilde[33] = aTilde[34] = aTilde[35] = 0.0;
// fill in the lower left quadrant with the calculated gradient values
aTilde[18] = gradnew(0,0);
aTilde[19] = gradnew(0,1);
aTilde[20] = gradnew(0,2);
aTilde[24] = gradnew(1,0);
aTilde[25] = gradnew(1,1);
aTilde[26] = gradnew(1,2);
aTilde[30] = gradnew(2,0);
aTilde[31] = gradnew(2,1);
aTilde[32] = gradnew(2,2);
for (Integer j = 0; j < 6; j++)
{
for (Integer k = 0; k < 6; k++)
{
element = j * 6 + k;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("------ deriv[%d] = %12.10f\n", (i6+element), aTilde[element]);
#endif
deriv[i6+element] = aTilde[element];
}
}
}
}
} // end for
}
#ifdef DEBUG_FIRST_CALL
if (firstCallFired == false)
{
if (body->GetName() == "Mars")
{
MessageInterface::ShowMessage(
" GravityField[%s <> %s] --> mu = %lf, origin = %s, [%.10lf %.10lf "
"%.10lf %.16lf %.16lf %.16lf]\n",
instanceName.c_str(), body->GetName().c_str(), mu,
targetCS->GetOriginName().c_str(),
deriv[0], deriv[1], deriv[2], deriv[3], deriv[4], deriv[5]);
firstCallFired = true;
}
}
#endif
return true;
}
