Andy K / Mbed 2 deprecated SOWB

Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 /*------------------------------------------------------------------*/