DateTime rtcGetTime(void)
{
DateTime tm;
if(movieMode == MOVIEMODE_INACTIVE) {
return DateTime::get_Now();
}
else {
//now, you might think it is silly to go through all these conniptions
//when we could just assume that there are 60fps and base the seconds on frameCounter/60
//but, we were imagining that one day we might need more precision
const u32 arm9rate_unitsperframe = 560190<<1;
const u32 arm9rate_unitspersecond = (u32)(arm9rate_unitsperframe * 59.8261);
u64 totalcycles = (u64)arm9rate_unitsperframe * currFrameCounter;
u64 totalseconds=totalcycles/arm9rate_unitspersecond;
DateTime timer = currMovieData.rtcStart;
return timer.AddSeconds(totalseconds);
}
}
int computeAtomResiduals( const gsl_vector* independentVariables,
void* parameters,
gsl_vector* residuals )
{
// Store parameters locally.
const Vector3 departurePosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->departurePosition;
const DateTime departureEpoch
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->departureEpoch;
const Vector3 targetPosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->targetPosition;
const Real timeOfFlight
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->timeOfFlight;
const Real earthGravitationalParameter
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->earthGravitationalParameter;
const Real earthMeanRadius
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->earthMeanRadius;
const Tle referenceTle
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->referenceTle;
const Real absoluteTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->absoluteTolerance;
const Real relativeTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->relativeTolerance;
const int maximumIterations
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->maximumIterations;
// Set Departure state [km; km/s].
std::vector< Real > departureVelocity( 3 );
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( independentVariables, i );
}
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
// Evaluate system of non-linear equations and store residuals.
gsl_vector_set( residuals, 0,
( arrivalState.Position( ).x - targetPosition[ 0 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 1,
( arrivalState.Position( ).y - targetPosition[ 1 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 2,
( arrivalState.Position( ).z - targetPosition[ 2 ] ) / earthMeanRadius );
return GSL_SUCCESS;
}
DateTime operator + (const DateTime &dt, const TimeDuration &span)
{
return dt.AddSeconds(span.GetSeconds());
}
const std::pair< Vector3, Vector3 > executeAtomSolver(
const Vector3& departurePosition,
const DateTime& departureEpoch,
const Vector3& arrivalPosition,
const Real timeOfFlight,
const Vector3& departureVelocityGuess,
std::string& solverStatusSummary,
int& numberOfIterations,
const Tle& referenceTle,
const Real earthGravitationalParameter,
const Real earthMeanRadius,
const Real absoluteTolerance,
const Real relativeTolerance,
const int maximumIterations )
{
// Set up parameters for residual function.
AtomParameters< Real, Vector3 > parameters( departurePosition,
departureEpoch,
arrivalPosition,
timeOfFlight,
earthGravitationalParameter,
earthMeanRadius,
referenceTle,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Set up residual function.
gsl_multiroot_function atomFunction
= {
&computeAtomResiduals< Real, Vector3 >, 3, ¶meters
};
// Set initial guess.
gsl_vector* initialGuess = gsl_vector_alloc( 3 );
for ( int i = 0; i < 3; i++ )
{
gsl_vector_set( initialGuess, i, departureVelocityGuess[ i ] );
}
// Set up solver type (derivative free).
const gsl_multiroot_fsolver_type* solverType = gsl_multiroot_fsolver_hybrids;
// Allocate memory for solver.
gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc( solverType, 3 );
// Set solver to use residual function with initial guess.
gsl_multiroot_fsolver_set( solver, &atomFunction, initialGuess );
// Declare current solver status and iteration counter.
int solverStatus = false;
int counter = 0;
// Set up buffer to store solver status summary table.
std::ostringstream summary;
// Print header for summary table to buffer.
summary << printAtomSolverStateTableHeader( );
do
{
// Print current state of solver for summary table.
summary << printAtomSolverState( counter, solver );
// Increment iteration counter.
++counter;
// Execute solver iteration.
solverStatus = gsl_multiroot_fsolver_iterate( solver );
// Check if solver is stuck; if it is stuck, break from loop.
if ( solverStatus )
{
std::cerr << "GSL solver status: " << solverStatus << std::endl;
std::cerr << summary.str( ) << std::endl;
std::cerr << std::endl;
throw std::runtime_error( "ERROR: Non-linear solver is stuck!" );
}
// Check if root has been found (within tolerance).
solverStatus = gsl_multiroot_test_delta(
solver->dx, solver->x, absoluteTolerance, relativeTolerance );
} while ( solverStatus == GSL_CONTINUE && counter < maximumIterations );
// Save number of iterations.
numberOfIterations = counter - 1;
// Print final status of solver to buffer.
summary << std::endl;
summary << "Status of non-linear solver: " << gsl_strerror( solverStatus ) << std::endl;
summary << std::endl;
// Write buffer contents to solver status summary string.
solverStatusSummary = summary.str( );
// Store final departure velocity.
Vector3 departureVelocity = departureVelocityGuess;
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( solver->x, i );
}
// Set departure state [km/s].
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements< Real >(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
Vector3 arrivalVelocity = departureVelocity;
arrivalVelocity[ 0 ] = arrivalState.Velocity( ).x;
arrivalVelocity[ 1 ] = arrivalState.Velocity( ).y;
arrivalVelocity[ 2 ] = arrivalState.Velocity( ).z;
// Free up memory.
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( initialGuess );
// Return departure and arrival velocities.
return std::make_pair< Vector3, Vector3 >( departureVelocity, arrivalVelocity );
}
