Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
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 /*------------------------------------------------------------------*/
Generated on Tue Jul 12 2022 18:05:35 by 1.7.2