sgp_math.c

00001 /*
00002  * Unit SGP_Math
00003  *       Author:  Dr TS Kelso
00004  * Original Version:  1991 Oct 30
00005  * Current Revision:  1998 Mar 17
00006  *          Version:  3.00
00007  *        Copyright:  1991-1998, All Rights Reserved
00008  *
00009  *   ported to C by:  Neoklis Kyriazis  April 9 2001
00010  */
00011 
00012 #include "sgp4sdp4.h"
00013 
00014 /* Returns sign of a double */
00015 int
00016 Sign(double arg)
00017 {
00018   if( arg > 0 )
00019     return( 1 );
00020   else if( arg < 0 )
00021     return( -1 );
00022   else
00023     return( 0 );
00024 } /* Function Sign*/
00025 
00026 /*------------------------------------------------------------------*/
00027 
00028 /* Returns square of a double */
00029 double
00030 Sqr(double arg)
00031 {
00032   return( arg*arg );
00033 } /* Function Sqr */
00034 
00035 /*------------------------------------------------------------------*/
00036 
00037 /* Returns cube of a double */
00038 double
00039 Cube(double arg)
00040 {
00041   return( arg*arg*arg );
00042 } /*Function Cube*/
00043 
00044 /*------------------------------------------------------------------*/
00045 
00046 /* Returns angle in radians from arg id degrees */
00047 double
00048 Radians(double arg)
00049 {
00050   return( arg*de2ra );
00051 } /*Function Radians*/
00052 
00053 /*------------------------------------------------------------------*/
00054 
00055 /* Returns angle in degrees from arg in rads */
00056 double
00057 Degrees(double arg)
00058 {
00059   return( arg/de2ra );
00060 } /*Function Degrees*/
00061 
00062 /*------------------------------------------------------------------*/
00063 
00064 /* Returns the arcsine of the argument */
00065 double
00066 ArcSin(double arg)
00067 {
00068   if( fabs(arg) >= 1 )
00069     return( Sign(arg)*pio2 );
00070   else
00071     return( atan(arg/sqrt(1-arg*arg)) );
00072 } /*Function ArcSin*/
00073 
00074 /*------------------------------------------------------------------*/
00075 
00076 /* Returns orccosine of rgument */
00077 double
00078 ArcCos(double arg)
00079 {
00080   return( pio2 - ArcSin(arg) );
00081 } /*Function ArcCos*/
00082 
00083 /*------------------------------------------------------------------*/
00084 
00085 /* Calculates scalar magnitude of a vector_t argument */
00086 void
00087 SgpMagnitude(vector_t *v)
00088 {
00089   v->w = sqrt(Sqr(v->x) + Sqr(v->y) + Sqr(v->z));
00090 } /*Procedure SgpMagnitude*/
00091 
00092 /*------------------------------------------------------------------*/
00093 
00094 /* Adds vectors v1 and v2 together to produce v3 */
00095 void
00096 Vec_Add(vector_t *v1, vector_t *v2, vector_t *v3)
00097 {
00098   v3->x = v1->x + v2->x;
00099   v3->y = v1->y + v2->y;
00100   v3->z = v1->z + v2->z;
00101 
00102   SgpMagnitude(v3);
00103 } /*Procedure Vec_Add*/
00104 
00105 /*------------------------------------------------------------------*/
00106 
00107 /* Subtracts vector v2 from v1 to produce v3 */
00108 void
00109 Vec_Sub(vector_t *v1, vector_t *v2, vector_t *v3)
00110 {
00111   v3->x = v1->x - v2->x;
00112   v3->y = v1->y - v2->y;
00113   v3->z = v1->z - v2->z;
00114 
00115   SgpMagnitude(v3);
00116 } /*Procedure Vec_Sub*/
00117 
00118 /*------------------------------------------------------------------*/
00119 
00120 /* Multiplies the vector v1 by the scalar k to produce the vector v2 */
00121 void
00122 Scalar_Multiply(double k, vector_t *v1, vector_t *v2)
00123 {
00124   v2->x = k * v1->x;
00125   v2->y = k * v1->y;
00126   v2->z = k * v1->z;
00127   v2->w = fabs(k) * v1->w;
00128 } /*Procedure Scalar_Multiply*/
00129 
00130 /*------------------------------------------------------------------*/
00131 
00132 /* Multiplies the vector v1 by the scalar k */
00133 void
00134 Scale_Vector(double k, vector_t *v)
00135 { 
00136   v->x *= k;
00137   v->y *= k;
00138   v->z *= k;
00139   SgpMagnitude(v);
00140 } /* Procedure Scale_Vector */
00141 
00142 /*------------------------------------------------------------------*/
00143 
00144 /* Returns the dot product of two vectors */
00145 double
00146 Dot(vector_t *v1, vector_t *v2)
00147 {
00148   return( v1->x*v2->x + v1->y*v2->y + v1->z*v2->z );
00149 }  /*Function Dot*/
00150 
00151 /*------------------------------------------------------------------*/
00152 
00153 /* Calculates the angle between vectors v1 and v2 */
00154 double
00155 Angle(vector_t *v1, vector_t *v2)
00156 {
00157   SgpMagnitude(v1);
00158   SgpMagnitude(v2);
00159   return( ArcCos(Dot(v1,v2)/(v1->w*v2->w)) );
00160 } /*Function Angle*/
00161 
00162 /*------------------------------------------------------------------*/
00163 
00164 /* Produces cross product of v1 and v2, and returns in v3 */
00165 void
00166 Cross(vector_t *v1, vector_t *v2 ,vector_t *v3)
00167 {
00168   v3->x = v1->y*v2->z - v1->z*v2->y;
00169   v3->y = v1->z*v2->x - v1->x*v2->z;
00170   v3->z = v1->x*v2->y - v1->y*v2->x;
00171   SgpMagnitude(v3);
00172 } /*Procedure Cross*/
00173 
00174 /*------------------------------------------------------------------*/
00175 
00176 /* Normalizes a vector */
00177 void
00178 Normalize( vector_t *v )
00179 {
00180   v->x /= v->w;
00181   v->y /= v->w;
00182   v->z /= v->w;
00183 } /*Procedure Normalize*/
00184 
00185 /*------------------------------------------------------------------*/
00186 
00187 /* Four-quadrant arctan function */
00188 double
00189 AcTan(double sinx, double cosx)
00190 {
00191   if(cosx == 0)
00192     {
00193       if(sinx > 0)
00194     return (pio2);
00195       else
00196     return (x3pio2);
00197     }
00198   else
00199     {
00200       if(cosx > 0)
00201     {
00202       if(sinx > 0)
00203         return ( atan(sinx/cosx) );
00204       else
00205         return ( twopi + atan(sinx/cosx) );
00206     }
00207       else
00208     return ( pi + atan(sinx/cosx) );
00209     }
00210 
00211 } /* Function AcTan */
00212 
00213 /*------------------------------------------------------------------*/
00214 
00215 /* Returns mod 2pi of argument */
00216 double
00217 FMod2p(double x)
00218 {
00219   int i;
00220   double ret_val;
00221 
00222   ret_val = x;
00223   i = ret_val/twopi;
00224   ret_val -= i*twopi;
00225   if (ret_val < 0) ret_val += twopi;
00226 
00227   return (ret_val);
00228 } /* fmod2p */
00229 
00230 /*------------------------------------------------------------------*/
00231 
00232 /* Returns arg1 mod arg2 */
00233 double
00234 Modulus(double arg1, double arg2)
00235 {
00236   int i;
00237   double ret_val;
00238 
00239   ret_val = arg1;
00240   i = ret_val/arg2;
00241   ret_val -= i*arg2;
00242   if (ret_val < 0) ret_val += arg2;
00243 
00244   return (ret_val);
00245 } /* modulus */
00246 
00247 /*------------------------------------------------------------------*/
00248 
00249 /* Returns fractional part of double argument */
00250 double
00251 Frac( double arg )
00252 {
00253   return( arg - floor(arg) );
00254 } /* Frac */
00255 
00256 /*------------------------------------------------------------------*/
00257 
00258 /* Returns argument rounded up to nearest integer */
00259 int
00260 Round( double arg )
00261 {
00262   return( (int) floor(arg + 0.5) );
00263 } /* Round */
00264 
00265 /*------------------------------------------------------------------*/
00266 
00267 /* Returns the floor integer of a double arguement, as double */
00268 double
00269 Int( double arg )
00270 {
00271   return( floor(arg) );
00272 } /* Int */
00273 
00274 /*------------------------------------------------------------------*/
00275 
00276 /* Converts the satellite's position and velocity  */
00277 /* vectors from normalised values to km and km/sec */ 
00278 void
00279 Convert_Sat_State( vector_t *pos, vector_t *vel )
00280 {
00281       Scale_Vector( xkmper, pos );
00282       Scale_Vector( xkmper*xmnpda/secday, vel );
00283 
00284 } /* Procedure Convert_Sat_State */
00285 
00286 /*------------------------------------------------------------------*/