Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
sgp4sdp4/sgp_math.c@0:0a841b89d614, 2010-10-11 (annotated)
- Committer:
- AjK
- Date:
- Mon Oct 11 10:34:55 2010 +0000
- Revision:
- 0:0a841b89d614
Totally Alpha quality as this project isn\t completed. Just publishing it as it answers many questions asked in the forums
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AjK | 0:0a841b89d614 | 1 | /* |
AjK | 0:0a841b89d614 | 2 | * Unit SGP_Math |
AjK | 0:0a841b89d614 | 3 | * Author: Dr TS Kelso |
AjK | 0:0a841b89d614 | 4 | * Original Version: 1991 Oct 30 |
AjK | 0:0a841b89d614 | 5 | * Current Revision: 1998 Mar 17 |
AjK | 0:0a841b89d614 | 6 | * Version: 3.00 |
AjK | 0:0a841b89d614 | 7 | * Copyright: 1991-1998, All Rights Reserved |
AjK | 0:0a841b89d614 | 8 | * |
AjK | 0:0a841b89d614 | 9 | * ported to C by: Neoklis Kyriazis April 9 2001 |
AjK | 0:0a841b89d614 | 10 | */ |
AjK | 0:0a841b89d614 | 11 | |
AjK | 0:0a841b89d614 | 12 | #include "sgp4sdp4.h" |
AjK | 0:0a841b89d614 | 13 | |
AjK | 0:0a841b89d614 | 14 | /* Returns sign of a double */ |
AjK | 0:0a841b89d614 | 15 | int |
AjK | 0:0a841b89d614 | 16 | Sign(double arg) |
AjK | 0:0a841b89d614 | 17 | { |
AjK | 0:0a841b89d614 | 18 | if( arg > 0 ) |
AjK | 0:0a841b89d614 | 19 | return( 1 ); |
AjK | 0:0a841b89d614 | 20 | else if( arg < 0 ) |
AjK | 0:0a841b89d614 | 21 | return( -1 ); |
AjK | 0:0a841b89d614 | 22 | else |
AjK | 0:0a841b89d614 | 23 | return( 0 ); |
AjK | 0:0a841b89d614 | 24 | } /* Function Sign*/ |
AjK | 0:0a841b89d614 | 25 | |
AjK | 0:0a841b89d614 | 26 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 27 | |
AjK | 0:0a841b89d614 | 28 | /* Returns square of a double */ |
AjK | 0:0a841b89d614 | 29 | double |
AjK | 0:0a841b89d614 | 30 | Sqr(double arg) |
AjK | 0:0a841b89d614 | 31 | { |
AjK | 0:0a841b89d614 | 32 | return( arg*arg ); |
AjK | 0:0a841b89d614 | 33 | } /* Function Sqr */ |
AjK | 0:0a841b89d614 | 34 | |
AjK | 0:0a841b89d614 | 35 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 36 | |
AjK | 0:0a841b89d614 | 37 | /* Returns cube of a double */ |
AjK | 0:0a841b89d614 | 38 | double |
AjK | 0:0a841b89d614 | 39 | Cube(double arg) |
AjK | 0:0a841b89d614 | 40 | { |
AjK | 0:0a841b89d614 | 41 | return( arg*arg*arg ); |
AjK | 0:0a841b89d614 | 42 | } /*Function Cube*/ |
AjK | 0:0a841b89d614 | 43 | |
AjK | 0:0a841b89d614 | 44 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 45 | |
AjK | 0:0a841b89d614 | 46 | /* Returns angle in radians from arg id degrees */ |
AjK | 0:0a841b89d614 | 47 | double |
AjK | 0:0a841b89d614 | 48 | Radians(double arg) |
AjK | 0:0a841b89d614 | 49 | { |
AjK | 0:0a841b89d614 | 50 | return( arg*de2ra ); |
AjK | 0:0a841b89d614 | 51 | } /*Function Radians*/ |
AjK | 0:0a841b89d614 | 52 | |
AjK | 0:0a841b89d614 | 53 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 54 | |
AjK | 0:0a841b89d614 | 55 | /* Returns angle in degrees from arg in rads */ |
AjK | 0:0a841b89d614 | 56 | double |
AjK | 0:0a841b89d614 | 57 | Degrees(double arg) |
AjK | 0:0a841b89d614 | 58 | { |
AjK | 0:0a841b89d614 | 59 | return( arg/de2ra ); |
AjK | 0:0a841b89d614 | 60 | } /*Function Degrees*/ |
AjK | 0:0a841b89d614 | 61 | |
AjK | 0:0a841b89d614 | 62 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 63 | |
AjK | 0:0a841b89d614 | 64 | /* Returns the arcsine of the argument */ |
AjK | 0:0a841b89d614 | 65 | double |
AjK | 0:0a841b89d614 | 66 | ArcSin(double arg) |
AjK | 0:0a841b89d614 | 67 | { |
AjK | 0:0a841b89d614 | 68 | if( fabs(arg) >= 1 ) |
AjK | 0:0a841b89d614 | 69 | return( Sign(arg)*pio2 ); |
AjK | 0:0a841b89d614 | 70 | else |
AjK | 0:0a841b89d614 | 71 | return( atan(arg/sqrt(1-arg*arg)) ); |
AjK | 0:0a841b89d614 | 72 | } /*Function ArcSin*/ |
AjK | 0:0a841b89d614 | 73 | |
AjK | 0:0a841b89d614 | 74 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 75 | |
AjK | 0:0a841b89d614 | 76 | /* Returns orccosine of rgument */ |
AjK | 0:0a841b89d614 | 77 | double |
AjK | 0:0a841b89d614 | 78 | ArcCos(double arg) |
AjK | 0:0a841b89d614 | 79 | { |
AjK | 0:0a841b89d614 | 80 | return( pio2 - ArcSin(arg) ); |
AjK | 0:0a841b89d614 | 81 | } /*Function ArcCos*/ |
AjK | 0:0a841b89d614 | 82 | |
AjK | 0:0a841b89d614 | 83 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 84 | |
AjK | 0:0a841b89d614 | 85 | /* Calculates scalar magnitude of a vector_t argument */ |
AjK | 0:0a841b89d614 | 86 | void |
AjK | 0:0a841b89d614 | 87 | SgpMagnitude(vector_t *v) |
AjK | 0:0a841b89d614 | 88 | { |
AjK | 0:0a841b89d614 | 89 | v->w = sqrt(Sqr(v->x) + Sqr(v->y) + Sqr(v->z)); |
AjK | 0:0a841b89d614 | 90 | } /*Procedure SgpMagnitude*/ |
AjK | 0:0a841b89d614 | 91 | |
AjK | 0:0a841b89d614 | 92 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 93 | |
AjK | 0:0a841b89d614 | 94 | /* Adds vectors v1 and v2 together to produce v3 */ |
AjK | 0:0a841b89d614 | 95 | void |
AjK | 0:0a841b89d614 | 96 | Vec_Add(vector_t *v1, vector_t *v2, vector_t *v3) |
AjK | 0:0a841b89d614 | 97 | { |
AjK | 0:0a841b89d614 | 98 | v3->x = v1->x + v2->x; |
AjK | 0:0a841b89d614 | 99 | v3->y = v1->y + v2->y; |
AjK | 0:0a841b89d614 | 100 | v3->z = v1->z + v2->z; |
AjK | 0:0a841b89d614 | 101 | |
AjK | 0:0a841b89d614 | 102 | SgpMagnitude(v3); |
AjK | 0:0a841b89d614 | 103 | } /*Procedure Vec_Add*/ |
AjK | 0:0a841b89d614 | 104 | |
AjK | 0:0a841b89d614 | 105 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 106 | |
AjK | 0:0a841b89d614 | 107 | /* Subtracts vector v2 from v1 to produce v3 */ |
AjK | 0:0a841b89d614 | 108 | void |
AjK | 0:0a841b89d614 | 109 | Vec_Sub(vector_t *v1, vector_t *v2, vector_t *v3) |
AjK | 0:0a841b89d614 | 110 | { |
AjK | 0:0a841b89d614 | 111 | v3->x = v1->x - v2->x; |
AjK | 0:0a841b89d614 | 112 | v3->y = v1->y - v2->y; |
AjK | 0:0a841b89d614 | 113 | v3->z = v1->z - v2->z; |
AjK | 0:0a841b89d614 | 114 | |
AjK | 0:0a841b89d614 | 115 | SgpMagnitude(v3); |
AjK | 0:0a841b89d614 | 116 | } /*Procedure Vec_Sub*/ |
AjK | 0:0a841b89d614 | 117 | |
AjK | 0:0a841b89d614 | 118 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 119 | |
AjK | 0:0a841b89d614 | 120 | /* Multiplies the vector v1 by the scalar k to produce the vector v2 */ |
AjK | 0:0a841b89d614 | 121 | void |
AjK | 0:0a841b89d614 | 122 | Scalar_Multiply(double k, vector_t *v1, vector_t *v2) |
AjK | 0:0a841b89d614 | 123 | { |
AjK | 0:0a841b89d614 | 124 | v2->x = k * v1->x; |
AjK | 0:0a841b89d614 | 125 | v2->y = k * v1->y; |
AjK | 0:0a841b89d614 | 126 | v2->z = k * v1->z; |
AjK | 0:0a841b89d614 | 127 | v2->w = fabs(k) * v1->w; |
AjK | 0:0a841b89d614 | 128 | } /*Procedure Scalar_Multiply*/ |
AjK | 0:0a841b89d614 | 129 | |
AjK | 0:0a841b89d614 | 130 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 131 | |
AjK | 0:0a841b89d614 | 132 | /* Multiplies the vector v1 by the scalar k */ |
AjK | 0:0a841b89d614 | 133 | void |
AjK | 0:0a841b89d614 | 134 | Scale_Vector(double k, vector_t *v) |
AjK | 0:0a841b89d614 | 135 | { |
AjK | 0:0a841b89d614 | 136 | v->x *= k; |
AjK | 0:0a841b89d614 | 137 | v->y *= k; |
AjK | 0:0a841b89d614 | 138 | v->z *= k; |
AjK | 0:0a841b89d614 | 139 | SgpMagnitude(v); |
AjK | 0:0a841b89d614 | 140 | } /* Procedure Scale_Vector */ |
AjK | 0:0a841b89d614 | 141 | |
AjK | 0:0a841b89d614 | 142 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 143 | |
AjK | 0:0a841b89d614 | 144 | /* Returns the dot product of two vectors */ |
AjK | 0:0a841b89d614 | 145 | double |
AjK | 0:0a841b89d614 | 146 | Dot(vector_t *v1, vector_t *v2) |
AjK | 0:0a841b89d614 | 147 | { |
AjK | 0:0a841b89d614 | 148 | return( v1->x*v2->x + v1->y*v2->y + v1->z*v2->z ); |
AjK | 0:0a841b89d614 | 149 | } /*Function Dot*/ |
AjK | 0:0a841b89d614 | 150 | |
AjK | 0:0a841b89d614 | 151 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 152 | |
AjK | 0:0a841b89d614 | 153 | /* Calculates the angle between vectors v1 and v2 */ |
AjK | 0:0a841b89d614 | 154 | double |
AjK | 0:0a841b89d614 | 155 | Angle(vector_t *v1, vector_t *v2) |
AjK | 0:0a841b89d614 | 156 | { |
AjK | 0:0a841b89d614 | 157 | SgpMagnitude(v1); |
AjK | 0:0a841b89d614 | 158 | SgpMagnitude(v2); |
AjK | 0:0a841b89d614 | 159 | return( ArcCos(Dot(v1,v2)/(v1->w*v2->w)) ); |
AjK | 0:0a841b89d614 | 160 | } /*Function Angle*/ |
AjK | 0:0a841b89d614 | 161 | |
AjK | 0:0a841b89d614 | 162 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 163 | |
AjK | 0:0a841b89d614 | 164 | /* Produces cross product of v1 and v2, and returns in v3 */ |
AjK | 0:0a841b89d614 | 165 | void |
AjK | 0:0a841b89d614 | 166 | Cross(vector_t *v1, vector_t *v2 ,vector_t *v3) |
AjK | 0:0a841b89d614 | 167 | { |
AjK | 0:0a841b89d614 | 168 | v3->x = v1->y*v2->z - v1->z*v2->y; |
AjK | 0:0a841b89d614 | 169 | v3->y = v1->z*v2->x - v1->x*v2->z; |
AjK | 0:0a841b89d614 | 170 | v3->z = v1->x*v2->y - v1->y*v2->x; |
AjK | 0:0a841b89d614 | 171 | SgpMagnitude(v3); |
AjK | 0:0a841b89d614 | 172 | } /*Procedure Cross*/ |
AjK | 0:0a841b89d614 | 173 | |
AjK | 0:0a841b89d614 | 174 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 175 | |
AjK | 0:0a841b89d614 | 176 | /* Normalizes a vector */ |
AjK | 0:0a841b89d614 | 177 | void |
AjK | 0:0a841b89d614 | 178 | Normalize( vector_t *v ) |
AjK | 0:0a841b89d614 | 179 | { |
AjK | 0:0a841b89d614 | 180 | v->x /= v->w; |
AjK | 0:0a841b89d614 | 181 | v->y /= v->w; |
AjK | 0:0a841b89d614 | 182 | v->z /= v->w; |
AjK | 0:0a841b89d614 | 183 | } /*Procedure Normalize*/ |
AjK | 0:0a841b89d614 | 184 | |
AjK | 0:0a841b89d614 | 185 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 186 | |
AjK | 0:0a841b89d614 | 187 | /* Four-quadrant arctan function */ |
AjK | 0:0a841b89d614 | 188 | double |
AjK | 0:0a841b89d614 | 189 | AcTan(double sinx, double cosx) |
AjK | 0:0a841b89d614 | 190 | { |
AjK | 0:0a841b89d614 | 191 | if(cosx == 0) |
AjK | 0:0a841b89d614 | 192 | { |
AjK | 0:0a841b89d614 | 193 | if(sinx > 0) |
AjK | 0:0a841b89d614 | 194 | return (pio2); |
AjK | 0:0a841b89d614 | 195 | else |
AjK | 0:0a841b89d614 | 196 | return (x3pio2); |
AjK | 0:0a841b89d614 | 197 | } |
AjK | 0:0a841b89d614 | 198 | else |
AjK | 0:0a841b89d614 | 199 | { |
AjK | 0:0a841b89d614 | 200 | if(cosx > 0) |
AjK | 0:0a841b89d614 | 201 | { |
AjK | 0:0a841b89d614 | 202 | if(sinx > 0) |
AjK | 0:0a841b89d614 | 203 | return ( atan(sinx/cosx) ); |
AjK | 0:0a841b89d614 | 204 | else |
AjK | 0:0a841b89d614 | 205 | return ( twopi + atan(sinx/cosx) ); |
AjK | 0:0a841b89d614 | 206 | } |
AjK | 0:0a841b89d614 | 207 | else |
AjK | 0:0a841b89d614 | 208 | return ( pi + atan(sinx/cosx) ); |
AjK | 0:0a841b89d614 | 209 | } |
AjK | 0:0a841b89d614 | 210 | |
AjK | 0:0a841b89d614 | 211 | } /* Function AcTan */ |
AjK | 0:0a841b89d614 | 212 | |
AjK | 0:0a841b89d614 | 213 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 214 | |
AjK | 0:0a841b89d614 | 215 | /* Returns mod 2pi of argument */ |
AjK | 0:0a841b89d614 | 216 | double |
AjK | 0:0a841b89d614 | 217 | FMod2p(double x) |
AjK | 0:0a841b89d614 | 218 | { |
AjK | 0:0a841b89d614 | 219 | int i; |
AjK | 0:0a841b89d614 | 220 | double ret_val; |
AjK | 0:0a841b89d614 | 221 | |
AjK | 0:0a841b89d614 | 222 | ret_val = x; |
AjK | 0:0a841b89d614 | 223 | i = ret_val/twopi; |
AjK | 0:0a841b89d614 | 224 | ret_val -= i*twopi; |
AjK | 0:0a841b89d614 | 225 | if (ret_val < 0) ret_val += twopi; |
AjK | 0:0a841b89d614 | 226 | |
AjK | 0:0a841b89d614 | 227 | return (ret_val); |
AjK | 0:0a841b89d614 | 228 | } /* fmod2p */ |
AjK | 0:0a841b89d614 | 229 | |
AjK | 0:0a841b89d614 | 230 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 231 | |
AjK | 0:0a841b89d614 | 232 | /* Returns arg1 mod arg2 */ |
AjK | 0:0a841b89d614 | 233 | double |
AjK | 0:0a841b89d614 | 234 | Modulus(double arg1, double arg2) |
AjK | 0:0a841b89d614 | 235 | { |
AjK | 0:0a841b89d614 | 236 | int i; |
AjK | 0:0a841b89d614 | 237 | double ret_val; |
AjK | 0:0a841b89d614 | 238 | |
AjK | 0:0a841b89d614 | 239 | ret_val = arg1; |
AjK | 0:0a841b89d614 | 240 | i = ret_val/arg2; |
AjK | 0:0a841b89d614 | 241 | ret_val -= i*arg2; |
AjK | 0:0a841b89d614 | 242 | if (ret_val < 0) ret_val += arg2; |
AjK | 0:0a841b89d614 | 243 | |
AjK | 0:0a841b89d614 | 244 | return (ret_val); |
AjK | 0:0a841b89d614 | 245 | } /* modulus */ |
AjK | 0:0a841b89d614 | 246 | |
AjK | 0:0a841b89d614 | 247 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 248 | |
AjK | 0:0a841b89d614 | 249 | /* Returns fractional part of double argument */ |
AjK | 0:0a841b89d614 | 250 | double |
AjK | 0:0a841b89d614 | 251 | Frac( double arg ) |
AjK | 0:0a841b89d614 | 252 | { |
AjK | 0:0a841b89d614 | 253 | return( arg - floor(arg) ); |
AjK | 0:0a841b89d614 | 254 | } /* Frac */ |
AjK | 0:0a841b89d614 | 255 | |
AjK | 0:0a841b89d614 | 256 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 257 | |
AjK | 0:0a841b89d614 | 258 | /* Returns argument rounded up to nearest integer */ |
AjK | 0:0a841b89d614 | 259 | int |
AjK | 0:0a841b89d614 | 260 | Round( double arg ) |
AjK | 0:0a841b89d614 | 261 | { |
AjK | 0:0a841b89d614 | 262 | return( (int) floor(arg + 0.5) ); |
AjK | 0:0a841b89d614 | 263 | } /* Round */ |
AjK | 0:0a841b89d614 | 264 | |
AjK | 0:0a841b89d614 | 265 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 266 | |
AjK | 0:0a841b89d614 | 267 | /* Returns the floor integer of a double arguement, as double */ |
AjK | 0:0a841b89d614 | 268 | double |
AjK | 0:0a841b89d614 | 269 | Int( double arg ) |
AjK | 0:0a841b89d614 | 270 | { |
AjK | 0:0a841b89d614 | 271 | return( floor(arg) ); |
AjK | 0:0a841b89d614 | 272 | } /* Int */ |
AjK | 0:0a841b89d614 | 273 | |
AjK | 0:0a841b89d614 | 274 | /*------------------------------------------------------------------*/ |
AjK | 0:0a841b89d614 | 275 | |
AjK | 0:0a841b89d614 | 276 | /* Converts the satellite's position and velocity */ |
AjK | 0:0a841b89d614 | 277 | /* vectors from normalised values to km and km/sec */ |
AjK | 0:0a841b89d614 | 278 | void |
AjK | 0:0a841b89d614 | 279 | Convert_Sat_State( vector_t *pos, vector_t *vel ) |
AjK | 0:0a841b89d614 | 280 | { |
AjK | 0:0a841b89d614 | 281 | Scale_Vector( xkmper, pos ); |
AjK | 0:0a841b89d614 | 282 | Scale_Vector( xkmper*xmnpda/secday, vel ); |
AjK | 0:0a841b89d614 | 283 | |
AjK | 0:0a841b89d614 | 284 | } /* Procedure Convert_Sat_State */ |
AjK | 0:0a841b89d614 | 285 | |
AjK | 0:0a841b89d614 | 286 | /*------------------------------------------------------------------*/ |