PaGMO  1.1.5
Astro_Functions.cpp
1 /*****************************************************************************
2  * Copyright (C) 2004-2015 The PaGMO development team, *
3  * Advanced Concepts Team (ACT), European Space Agency (ESA) *
4  * *
5  * https://github.com/esa/pagmo *
6  * *
7  * act@esa.int *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the *
21  * Free Software Foundation, Inc., *
22  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
23  *****************************************************************************/
24 
25 #include <cmath>
26 #include <boost/math/special_functions/acosh.hpp>
27 #include <boost/math/special_functions/asinh.hpp>
28 
29 #include "Astro_Functions.h"
30 #include "ZeroFinder.h"
31 
32 using namespace std;
33 
34 class CZF : public ZeroFinder::Function1D
35 {
36 public:
37  CZF(const double &a, const double &b):Function1D(a,b) {}
38  double operator()(const double &x) const
39  {
40  return (p1*tan(x) - log(tan(0.5*x + M_PI_4)) - p2);
41  }
42 };
43 
44 double Mean2Eccentric (const double &M, const double &e)
45 {
46 
47  static const double tolerance = 1e-13;
48  int n_of_it = 0; // Number of iterations
49  double Eccentric,Ecc_New;
50  double err = 1.0;
51 
52 
53 
54  if (e < 1.0) {
55  Eccentric = M + e* cos(M); // Initial guess
56  while ( (err > tolerance) && (n_of_it < 100))
57  {
58  Ecc_New = Eccentric - (Eccentric - e* sin(Eccentric) -M )/(1.0 - e * cos(Eccentric));
59  err = fabs(Eccentric - Ecc_New);
60  Eccentric = Ecc_New;
61  n_of_it++;
62  }
63  } else {
64  CZF FF(e,M); // function to find its zero point
65  ZeroFinder::FZero fz(-M_PI_2 + 1e-8, M_PI_2 - 1e-8);
66  Ecc_New = fz.FindZero(FF);
67  Eccentric = Ecc_New;
68  }
69  return Eccentric;
70 }
71 
72 
73 
74 void Conversion (const double *E,
75  double *pos,
76  double *vel,
77  const double &mu)
78 {
79  double a,e,i,omg,omp,theta;
80  double b,n;
81  double X_per[3],X_dotper[3];
82  double R[3][3];
83 
84  a = E[0];
85  e = E[1];
86  i = E[2];
87  omg = E[3];
88  omp = E[4];
89  theta = E[5];
90 
91  b = a * sqrt (1 - e*e);
92  n = sqrt (mu/pow(a,3));
93 
94  const double sin_theta = sin(theta);
95  const double cos_theta = cos(theta);
96 
97  X_per[0] = a * (cos_theta - e);
98  X_per[1] = b * sin_theta;
99 
100  X_dotper[0] = -(a * n * sin_theta)/(1 - e * cos_theta);
101  X_dotper[1] = (b * n * cos_theta)/(1 - e * cos_theta);
102 
103  const double cosomg = cos(omg);
104  const double cosomp = cos(omp);
105  const double sinomg = sin(omg);
106  const double sinomp = sin(omp);
107  const double cosi = cos(i);
108  const double sini = sin(i);
109 
110  R[0][0] = cosomg*cosomp-sinomg*sinomp*cosi;
111  R[0][1] = -cosomg*sinomp-sinomg*cosomp*cosi;
112 
113  R[1][0] = sinomg*cosomp+cosomg*sinomp*cosi;
114  R[1][1] = -sinomg*sinomp+cosomg*cosomp*cosi;
115 
116  R[2][0] = sinomp*sini;
117  R[2][1] = cosomp*sini;
118 
119  // evaluate position and velocity
120  for (int i = 0;i < 3;i++)
121  {
122  pos[i] = 0;
123  vel[i] = 0;
124  for (int j = 0;j < 2;j++)
125  {
126  pos[i] += R[i][j] * X_per[j];
127  vel[i] += R[i][j] * X_dotper[j];
128  }
129  }
130  return;
131 }
132 
133 
134 double norm(const double *vet1, const double *vet2)
135 {
136  double Vin = 0;
137  for (int i = 0; i < 3; i++)
138  {
139  Vin += (vet1[i] - vet2[i])*(vet1[i] - vet2[i]);
140  }
141  return sqrt(Vin);
142 }
143 
144 
145 double norm2(const double *vet1)
146 {
147  double temp = 0.0;
148  for (int i = 0; i < 3; i++) {
149  temp += vet1[i] * vet1[i];
150  }
151  return sqrt(temp);
152 }
153 
154 
155 //subfunction that evaluates vector product
156 void vett(const double *vet1, const double *vet2, double *prod )
157 {
158  prod[0]=(vet1[1]*vet2[2]-vet1[2]*vet2[1]);
159  prod[1]=(vet1[2]*vet2[0]-vet1[0]*vet2[2]);
160  prod[2]=(vet1[0]*vet2[1]-vet1[1]*vet2[0]);
161 }
162 
163 double x2tof(const double &x,const double &s,const double &c,const int &lw)
164 {
165  double am,a,alfa,beta;
166 
167  am = s/2;
168  a = am/(1-x*x);
169  if (x < 1)//ellpise
170  {
171  beta = 2 * asin (sqrt((s - c)/(2*a)));
172  if (lw) beta = -beta;
173  alfa = 2 * acos(x);
174  }
175  else
176  {
177  alfa = 2 * boost::math::acosh(x);
178  beta = 2 * boost::math::asinh(sqrt ((s - c)/(-2 * a)));
179  if (lw) beta = -beta;
180  }
181 
182  if (a > 0)
183  {
184  return (a * sqrt (a)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
185  }
186  else
187  {
188  return (-a * sqrt(-a)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
189  }
190 
191 }
192 
193 
194 // Subfunction that evaluates the time of flight as a function of x
195 double tofabn(const double &sigma,const double &alfa,const double &beta)
196 {
197  if (sigma > 0)
198  {
199  return (sigma * sqrt (sigma)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
200  }
201  else
202  {
203  return (-sigma * sqrt(-sigma)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
204  }
205 }
206 
207 // subfunction that evaluates unit vectors
208 void vers(const double *V_in, double *Ver_out)
209 {
210  double v_mod = 0;
211  int i;
212 
213  for (i = 0;i < 3;i++)
214  {
215  v_mod += V_in[i]*V_in[i];
216  }
217 
218  double sqrtv_mod = sqrt(v_mod);
219 
220  for (i = 0;i < 3;i++)
221  {
222  Ver_out[i] = V_in[i]/sqrtv_mod;
223  }
224 }
225 
226 
227 
228 
STL namespace.