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  {
 17      if (arg > 0)
 18          return (1);
 19      else if (arg < 0)
 20          return (-1);
 21      else
 22          return (0);
 23  }
 24  
 25  /* Returns square of a double */
 26  double Sqr(double arg)
 27  {
 28      return (arg * arg);
 29  }
 30  
 31  /* Returns cube of a double */
 32  double Cube(double arg)
 33  {
 34      return (arg * arg * arg);
 35  }
 36  
 37  /* Returns angle in radians from arg id degrees */
 38  double Radians(double arg)
 39  {
 40      return (arg * de2ra);
 41  }
 42  
 43  /* Returns angle in degrees from arg in rads */
 44  double Degrees(double arg)
 45  {
 46      return (arg / de2ra);
 47  }
 48  
 49  /* Returns the arcsine of the argument */
 50  double ArcSin(double arg)
 51  {
 52      if (fabs(arg) >= 1)
 53          return (Sign(arg) * pio2);
 54      else
 55          return (atan(arg / sqrt(1 - arg * arg)));
 56  }
 57  
 58  /* Returns orccosine of rgument */
 59  double ArcCos(double arg)
 60  {
 61      return (pio2 - ArcSin(arg));
 62  }
 63  
 64  /* Calculates scalar magnitude of a vector_t argument */
 65  void Magnitude(vector_t * v)
 66  {
 67      v->w = sqrt(Sqr(v->x) + Sqr(v->y) + Sqr(v->z));
 68  }
 69  
 70  /* Adds vectors v1 and v2 together to produce v3 */
 71  void Vec_Add(vector_t * v1, vector_t * v2, vector_t * v3)
 72  {
 73      v3->x = v1->x + v2->x;
 74      v3->y = v1->y + v2->y;
 75      v3->z = v1->z + v2->z;
 76  
 77      Magnitude(v3);
 78  }
 79  
 80  /* Subtracts vector v2 from v1 to produce v3 */
 81  void Vec_Sub(vector_t * v1, vector_t * v2, vector_t * v3)
 82  {
 83      v3->x = v1->x - v2->x;
 84      v3->y = v1->y - v2->y;
 85      v3->z = v1->z - v2->z;
 86  
 87      Magnitude(v3);
 88  }
 89  
 90  /* Multiplies the vector v1 by the scalar k to produce the vector v2 */
 91  void Scalar_Multiply(double k, vector_t * v1, vector_t * v2)
 92  {
 93      v2->x = k * v1->x;
 94      v2->y = k * v1->y;
 95      v2->z = k * v1->z;
 96      v2->w = fabs(k) * v1->w;
 97  }
 98  
 99  /* Multiplies the vector v1 by the scalar k */
100  void Scale_Vector(double k, vector_t * v)
101  {
102      v->x *= k;
103      v->y *= k;
104      v->z *= k;
105      Magnitude(v);
106  }
107  
108  /* Returns the dot product of two vectors */
109  double Dot(vector_t * v1, vector_t * v2)
110  {
111      return (v1->x * v2->x + v1->y * v2->y + v1->z * v2->z);
112  }
113  
114  /* Calculates the angle between vectors v1 and v2 */
115  double Angle(vector_t * v1, vector_t * v2)
116  {
117      Magnitude(v1);
118      Magnitude(v2);
119      return (ArcCos(Dot(v1, v2) / (v1->w * v2->w)));
120  }
121  
122  /* Produces cross product of v1 and v2, and returns in v3 */
123  void Cross(vector_t * v1, vector_t * v2, vector_t * v3)
124  {
125      v3->x = v1->y * v2->z - v1->z * v2->y;
126      v3->y = v1->z * v2->x - v1->x * v2->z;
127      v3->z = v1->x * v2->y - v1->y * v2->x;
128      Magnitude(v3);
129  }
130  
131  /* Normalizes a vector */
132  void Normalize(vector_t * v)
133  {
134      v->x /= v->w;
135      v->y /= v->w;
136      v->z /= v->w;
137  }
138  
139  /* Four-quadrant arctan function */
140  double AcTan(double sinx, double cosx)
141  {
142      if (cosx == 0)
143      {
144          if (sinx > 0)
145              return (pio2);
146          else
147              return (x3pio2);
148      }
149      else
150      {
151          if (cosx > 0)
152          {
153              if (sinx > 0)
154                  return (atan(sinx / cosx));
155              else
156                  return (twopi + atan(sinx / cosx));
157          }
158          else
159              return (pi + atan(sinx / cosx));
160      }
161  
162  }
163  
164  /* Returns mod 2pi of argument */
165  double FMod2p(double x)
166  {
167      int             i;
168      double          ret_val;
169  
170      ret_val = x;
171      i = ret_val / twopi;
172      ret_val -= i * twopi;
173      if (ret_val < 0)
174          ret_val += twopi;
175  
176      return (ret_val);
177  }
178  
179  /* Returns arg1 mod arg2 */
180  double Modulus(double arg1, double arg2)
181  {
182      int             i;
183      double          ret_val;
184  
185      ret_val = arg1;
186      i = ret_val / arg2;
187      ret_val -= i * arg2;
188      if (ret_val < 0)
189          ret_val += arg2;
190  
191      return (ret_val);
192  }
193  
194  /* Returns fractional part of double argument */
195  double Frac(double arg)
196  {
197      return (arg - floor(arg));
198  }
199  
200  /* Returns argument rounded up to nearest integer */
201  int Round(double arg)
202  {
203      return ((int)floor(arg + 0.5));
204  }
205  
206  /* Returns the floor integer of a double argument, as double */
207  double Int(double arg)
208  {
209      return (floor(arg));
210  }
211  
212  /* Converts the satellite's position and velocity  */
213  /* vectors from normalised values to km and km/sec */
214  void Convert_Sat_State(vector_t * pos, vector_t * vel)
215  {
216      Scale_Vector(xkmper, pos);
217      Scale_Vector(xkmper * xmnpda / secday, vel);
218  }