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