//------------------------------------------------------------------------------ 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; }