mirror of
https://github.com/nasa/trick.git
synced 2024-12-21 06:03:10 +00:00
214 lines
6.7 KiB
C++
214 lines
6.7 KiB
C++
#include "Integrator.hh"
|
|
#include <math.h>
|
|
#include <iostream>
|
|
|
|
// This is from matrix_macros.h in Trick.
|
|
/* matrix times vector : prod[i] = mat[i][j] * vect[i] */
|
|
#define MxV( prod , mat , vect ) { \
|
|
prod[0] = mat[0][0] * vect[0] + mat[0][1] * vect[1] + mat[0][2] * vect[2] ; \
|
|
prod[1] = mat[1][0] * vect[0] + mat[1][1] * vect[1] + mat[1][2] * vect[2] ; \
|
|
prod[2] = mat[2][0] * vect[0] + mat[2][1] * vect[1] + mat[2][2] * vect[2] ; \
|
|
}
|
|
|
|
// GRAVITATIONAL_CONSTANT (Nm2/kg2)
|
|
#define GRAVITATIONAL_CONSTANT 6.67e-11
|
|
// LUNAR_MASS (kg)
|
|
#define LUNAR_MASS 7.36e22
|
|
// LUNAR RADIUS (m)
|
|
#define LUNAR_RADIUS 1737400.0
|
|
|
|
#define RADIANS_PER_DEGREE 0.0174532925
|
|
|
|
typedef struct {
|
|
double mass;
|
|
double pos[3];
|
|
double vel[3];
|
|
double acc[3];
|
|
} SATELLITE;
|
|
|
|
typedef struct {
|
|
double radius;
|
|
double mass;
|
|
} PLANET;
|
|
|
|
typedef struct {
|
|
double semi_major_axis; /* (m) */
|
|
double eccentricity; /* (--) */
|
|
double inclination; /* (r) */
|
|
double argument_of_perigee; /* (r) */
|
|
double right_ascension; /* (r) */
|
|
double true_anomaly; /* (r) */
|
|
} ORBITAL_ELEMENTS;
|
|
|
|
void calc_gravity( SATELLITE * S,
|
|
PLANET * P,
|
|
double * G ) {
|
|
|
|
double distance;
|
|
double pos_hat[3];
|
|
double g_force_mag;
|
|
|
|
// Calculate Gravitational Force
|
|
distance = sqrt( S->pos[0] * S->pos[0] +
|
|
S->pos[1] * S->pos[1] +
|
|
S->pos[2] * S->pos[2] );
|
|
|
|
pos_hat[0] = S->pos[0] / distance ;
|
|
pos_hat[1] = S->pos[1] / distance ;
|
|
pos_hat[2] = S->pos[2] / distance ;
|
|
|
|
g_force_mag = ( GRAVITATIONAL_CONSTANT * P->mass * S->mass ) / ( distance * distance ) ;
|
|
|
|
G[0] = g_force_mag * - pos_hat[0] ;
|
|
G[1] = g_force_mag * - pos_hat[1] ;
|
|
G[2] = g_force_mag * - pos_hat[2] ;
|
|
|
|
}
|
|
|
|
//
|
|
// Reference: ISS SIGI Attitude Processor Orbit Equations EG2/Rodolfo Gonzalez 20 Aug 2003
|
|
void orb_elem_to_intl( ORBITAL_ELEMENTS * O, /* IN */
|
|
PLANET * P, /* IN */
|
|
double * R_intl, /* OUT */
|
|
double * V_intl ) { /* OUT */
|
|
|
|
double ra, wp, in ;
|
|
double M_pci_perif[3][3]; /* Perifocal --> Inertial transform */
|
|
double semi_latus_rectum;
|
|
double r_magnitude;
|
|
double r_perifocal[3];
|
|
double v_perifocal[3];
|
|
double v_mag;
|
|
|
|
semi_latus_rectum = O->semi_major_axis * ( 1.0 - O->eccentricity * O->eccentricity );
|
|
|
|
r_magnitude = semi_latus_rectum / (1.0 + O->eccentricity * cos( O->true_anomaly ));
|
|
|
|
r_perifocal[0] = r_magnitude * cos( O->true_anomaly);
|
|
r_perifocal[1] = r_magnitude * sin( O->true_anomaly);
|
|
r_perifocal[2] = 0;
|
|
|
|
v_mag = sqrt( GRAVITATIONAL_CONSTANT * P->mass / semi_latus_rectum);
|
|
|
|
v_perifocal[0] = v_mag * (-sin(O->true_anomaly));
|
|
v_perifocal[1] = v_mag * (O->eccentricity + cos(O->true_anomaly));
|
|
v_perifocal[2] = 0;
|
|
|
|
ra = O->right_ascension;
|
|
wp = O->argument_of_perigee;
|
|
in = O->inclination;
|
|
|
|
M_pci_perif[0][0] = cos(ra)*cos(wp) - sin(ra)*cos(in)*sin(wp) ;
|
|
M_pci_perif[0][1] = -cos(ra)*sin(wp) - sin(ra)*cos(in)*cos(wp) ;
|
|
M_pci_perif[0][2] = sin(ra)*sin(in);
|
|
|
|
M_pci_perif[1][0] = sin(ra)*cos(wp) + cos(ra)*cos(in)*sin(wp) ;
|
|
M_pci_perif[1][1] = -sin(ra)*sin(wp) + cos(ra)*cos(in)*cos(wp) ;
|
|
M_pci_perif[1][2] = -cos(ra)*sin(in) ;
|
|
|
|
M_pci_perif[2][0] = sin(in)*sin(wp) ;
|
|
M_pci_perif[2][1] = sin(in)*cos(wp) ;
|
|
M_pci_perif[2][2] = cos(in) ;
|
|
|
|
MxV( R_intl , M_pci_perif , r_perifocal) ;
|
|
MxV( V_intl , M_pci_perif , v_perifocal) ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void deriv(SATELLITE *S, PLANET *P) {
|
|
|
|
double gforce[3];
|
|
|
|
calc_gravity(S, P, gforce );
|
|
|
|
S->acc[0] = gforce[0] / S->mass ;
|
|
S->acc[1] = gforce[1] / S->mass ;
|
|
S->acc[2] = gforce[2] / S->mass ;
|
|
|
|
}
|
|
|
|
int main(int argc, const char* argv[]) {
|
|
|
|
SATELLITE satellite;
|
|
PLANET planet;
|
|
ORBITAL_ELEMENTS orbital_elements;
|
|
|
|
long tick = 0;
|
|
double seconds_per_tick = 1.0;
|
|
double sim_time = 0.0;
|
|
|
|
// Initialization
|
|
satellite.mass = 100;
|
|
planet.mass = LUNAR_MASS;
|
|
planet.radius = LUNAR_RADIUS;
|
|
|
|
orbital_elements.semi_major_axis = 2000000; // meters
|
|
orbital_elements.eccentricity = 0.2;
|
|
orbital_elements.inclination = 5.0 * RADIANS_PER_DEGREE;
|
|
orbital_elements.argument_of_perigee = 0.0;
|
|
orbital_elements.right_ascension = 0.0; // radians
|
|
orbital_elements.true_anomaly = 0.0; // radians
|
|
|
|
orb_elem_to_intl( &orbital_elements,
|
|
&planet,
|
|
satellite.pos,
|
|
satellite.vel);
|
|
|
|
Trick::Integrator *I = getIntegrator( Euler, 6, 1.0);
|
|
|
|
do { // sim loop
|
|
sim_time = tick * seconds_per_tick ;
|
|
std::cout << "sim_time = " << sim_time << std::endl;
|
|
|
|
do { // Integration Loop
|
|
|
|
I->time = sim_time;
|
|
|
|
std::cout << "step #" << I->intermediate_step << std::endl; std::cout.flush();
|
|
|
|
std::cout << "deriv ..."; std::cout.flush();
|
|
deriv( &satellite, &planet);
|
|
std::cout << "done." << std::endl; std::cout.flush();
|
|
|
|
std::cout << "loading state ..."; std::cout.flush();
|
|
I->state[0] = satellite.pos[0];
|
|
I->state[1] = satellite.pos[1];
|
|
I->state[2] = satellite.pos[2];
|
|
I->state[3] = satellite.vel[0];
|
|
I->state[4] = satellite.vel[1];
|
|
I->state[5] = satellite.vel[2];
|
|
std::cout << "done." << std::endl; std::cout.flush();
|
|
|
|
std::cout << "loading state deriv ..."; std::cout.flush();
|
|
I->deriv[I->intermediate_step][0] = satellite.vel[0];
|
|
I->deriv[I->intermediate_step][1] = satellite.vel[1];
|
|
I->deriv[I->intermediate_step][2] = satellite.vel[2];
|
|
I->deriv[I->intermediate_step][3] = satellite.acc[0];
|
|
I->deriv[I->intermediate_step][4] = satellite.acc[1];
|
|
I->deriv[I->intermediate_step][5] = satellite.acc[2];
|
|
std::cout << "done." << std::endl; std::cout.flush();
|
|
|
|
std::cout << "integrate" << std::endl; std::cout.flush();
|
|
I->integrate();
|
|
std::cout << "done." << std::endl; std::cout.flush();
|
|
|
|
satellite.pos[0] = I->state_ws[I->intermediate_step][0];
|
|
satellite.pos[1] = I->state_ws[I->intermediate_step][1];
|
|
satellite.pos[2] = I->state_ws[I->intermediate_step][2];
|
|
satellite.vel[0] = I->state_ws[I->intermediate_step][3];
|
|
satellite.vel[1] = I->state_ws[I->intermediate_step][4];
|
|
satellite.vel[2] = I->state_ws[I->intermediate_step][5];
|
|
|
|
} while ( I->intermediate_step);
|
|
|
|
std::cout << "(" << satellite.pos[0] <<","<< satellite.pos[1] <<","<< satellite.pos[2] << ")" << std::endl;
|
|
|
|
tick++;
|
|
|
|
} while (sim_time < 10.0);
|
|
}
|