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