Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
Diff: sgp4sdp4/sgp_math.c
- Revision:
- 0:0a841b89d614
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sgp4sdp4/sgp_math.c Mon Oct 11 10:34:55 2010 +0000 @@ -0,0 +1,286 @@ +/* + * Unit SGP_Math + * Author: Dr TS Kelso + * Original Version: 1991 Oct 30 + * Current Revision: 1998 Mar 17 + * Version: 3.00 + * Copyright: 1991-1998, All Rights Reserved + * + * ported to C by: Neoklis Kyriazis April 9 2001 + */ + +#include "sgp4sdp4.h" + +/* Returns sign of a double */ +int +Sign(double arg) +{ + if( arg > 0 ) + return( 1 ); + else if( arg < 0 ) + return( -1 ); + else + return( 0 ); +} /* Function Sign*/ + +/*------------------------------------------------------------------*/ + +/* Returns square of a double */ +double +Sqr(double arg) +{ + return( arg*arg ); +} /* Function Sqr */ + +/*------------------------------------------------------------------*/ + +/* Returns cube of a double */ +double +Cube(double arg) +{ + return( arg*arg*arg ); +} /*Function Cube*/ + +/*------------------------------------------------------------------*/ + +/* Returns angle in radians from arg id degrees */ +double +Radians(double arg) +{ + return( arg*de2ra ); +} /*Function Radians*/ + +/*------------------------------------------------------------------*/ + +/* Returns angle in degrees from arg in rads */ +double +Degrees(double arg) +{ + return( arg/de2ra ); +} /*Function Degrees*/ + +/*------------------------------------------------------------------*/ + +/* Returns the arcsine of the argument */ +double +ArcSin(double arg) +{ + if( fabs(arg) >= 1 ) + return( Sign(arg)*pio2 ); + else + return( atan(arg/sqrt(1-arg*arg)) ); +} /*Function ArcSin*/ + +/*------------------------------------------------------------------*/ + +/* Returns orccosine of rgument */ +double +ArcCos(double arg) +{ + return( pio2 - ArcSin(arg) ); +} /*Function ArcCos*/ + +/*------------------------------------------------------------------*/ + +/* Calculates scalar magnitude of a vector_t argument */ +void +SgpMagnitude(vector_t *v) +{ + v->w = sqrt(Sqr(v->x) + Sqr(v->y) + Sqr(v->z)); +} /*Procedure SgpMagnitude*/ + +/*------------------------------------------------------------------*/ + +/* Adds vectors v1 and v2 together to produce v3 */ +void +Vec_Add(vector_t *v1, vector_t *v2, vector_t *v3) +{ + v3->x = v1->x + v2->x; + v3->y = v1->y + v2->y; + v3->z = v1->z + v2->z; + + SgpMagnitude(v3); +} /*Procedure Vec_Add*/ + +/*------------------------------------------------------------------*/ + +/* Subtracts vector v2 from v1 to produce v3 */ +void +Vec_Sub(vector_t *v1, vector_t *v2, vector_t *v3) +{ + v3->x = v1->x - v2->x; + v3->y = v1->y - v2->y; + v3->z = v1->z - v2->z; + + SgpMagnitude(v3); +} /*Procedure Vec_Sub*/ + +/*------------------------------------------------------------------*/ + +/* Multiplies the vector v1 by the scalar k to produce the vector v2 */ +void +Scalar_Multiply(double k, vector_t *v1, vector_t *v2) +{ + v2->x = k * v1->x; + v2->y = k * v1->y; + v2->z = k * v1->z; + v2->w = fabs(k) * v1->w; +} /*Procedure Scalar_Multiply*/ + +/*------------------------------------------------------------------*/ + +/* Multiplies the vector v1 by the scalar k */ +void +Scale_Vector(double k, vector_t *v) +{ + v->x *= k; + v->y *= k; + v->z *= k; + SgpMagnitude(v); +} /* Procedure Scale_Vector */ + +/*------------------------------------------------------------------*/ + +/* Returns the dot product of two vectors */ +double +Dot(vector_t *v1, vector_t *v2) +{ + return( v1->x*v2->x + v1->y*v2->y + v1->z*v2->z ); +} /*Function Dot*/ + +/*------------------------------------------------------------------*/ + +/* Calculates the angle between vectors v1 and v2 */ +double +Angle(vector_t *v1, vector_t *v2) +{ + SgpMagnitude(v1); + SgpMagnitude(v2); + return( ArcCos(Dot(v1,v2)/(v1->w*v2->w)) ); +} /*Function Angle*/ + +/*------------------------------------------------------------------*/ + +/* Produces cross product of v1 and v2, and returns in v3 */ +void +Cross(vector_t *v1, vector_t *v2 ,vector_t *v3) +{ + v3->x = v1->y*v2->z - v1->z*v2->y; + v3->y = v1->z*v2->x - v1->x*v2->z; + v3->z = v1->x*v2->y - v1->y*v2->x; + SgpMagnitude(v3); +} /*Procedure Cross*/ + +/*------------------------------------------------------------------*/ + +/* Normalizes a vector */ +void +Normalize( vector_t *v ) +{ + v->x /= v->w; + v->y /= v->w; + v->z /= v->w; +} /*Procedure Normalize*/ + +/*------------------------------------------------------------------*/ + +/* Four-quadrant arctan function */ +double +AcTan(double sinx, double cosx) +{ + if(cosx == 0) + { + if(sinx > 0) + return (pio2); + else + return (x3pio2); + } + else + { + if(cosx > 0) + { + if(sinx > 0) + return ( atan(sinx/cosx) ); + else + return ( twopi + atan(sinx/cosx) ); + } + else + return ( pi + atan(sinx/cosx) ); + } + +} /* Function AcTan */ + +/*------------------------------------------------------------------*/ + +/* Returns mod 2pi of argument */ +double +FMod2p(double x) +{ + int i; + double ret_val; + + ret_val = x; + i = ret_val/twopi; + ret_val -= i*twopi; + if (ret_val < 0) ret_val += twopi; + + return (ret_val); +} /* fmod2p */ + +/*------------------------------------------------------------------*/ + +/* Returns arg1 mod arg2 */ +double +Modulus(double arg1, double arg2) +{ + int i; + double ret_val; + + ret_val = arg1; + i = ret_val/arg2; + ret_val -= i*arg2; + if (ret_val < 0) ret_val += arg2; + + return (ret_val); +} /* modulus */ + +/*------------------------------------------------------------------*/ + +/* Returns fractional part of double argument */ +double +Frac( double arg ) +{ + return( arg - floor(arg) ); +} /* Frac */ + +/*------------------------------------------------------------------*/ + +/* Returns argument rounded up to nearest integer */ +int +Round( double arg ) +{ + return( (int) floor(arg + 0.5) ); +} /* Round */ + +/*------------------------------------------------------------------*/ + +/* Returns the floor integer of a double arguement, as double */ +double +Int( double arg ) +{ + return( floor(arg) ); +} /* Int */ + +/*------------------------------------------------------------------*/ + +/* Converts the satellite's position and velocity */ +/* vectors from normalised values to km and km/sec */ +void +Convert_Sat_State( vector_t *pos, vector_t *vel ) +{ + Scale_Vector( xkmper, pos ); + Scale_Vector( xkmper*xmnpda/secday, vel ); + +} /* Procedure Convert_Sat_State */ + +/*------------------------------------------------------------------*/