Swarm-NG  1.1
kepler.cpp
Go to the documentation of this file.
1 /*************************************************************************
2  * Copyright (C) 2009-2010 by Eric Ford & the Swarm-NG Development Team *
3  * *
4  * This program is free software; you can redistribute it and/or modify *
5  * it under the terms of the GNU General Public License as published by *
6  * the Free Software Foundation; either version 3 of the License. *
7  * *
8  * This program is distributed in the hope that it will be useful, *
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
11  * GNU General Public License for more details. *
12  * *
13  * You should have received a copy of the GNU General Public License *
14  * along with this program; if not, write to the *
15  * Free Software Foundation, Inc., *
16  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
17  ************************************************************************/
18 
24 #include "kepler.hpp"
25 #include <math.h>
26 #include <assert.h>
27 
28 // Code in this file largely adapted from Mercury.
29 
30 double improve_mean_to_eccentric_annomaly_guess(const double e, const double M, const double x)
31 {
32  // Based on Mercury
33  double sx, cx;
34  sincos(x,&sx,&cx);
35  // double sx = sin(x);
36  // double cx = cos(x);
37  double es = e*sx;
38  double ec = e*cx;
39  double f = x-es-M;
40  double fp = 1.-ec;
41  double fpp = es;
42  double fppp = ec;
43  double dx = -f/fp;
44  dx = -f/(fp+dx*fpp/2.);
45  dx = -f/(fp+dx*fpp/2.+dx*dx*fppp/6.);
46  return x+dx;
47 };
48 
49 double mean_to_eccentric_annomaly(const double e, double M)
50 {
51  // Based on Mercury
52  const int ORBEL_EHIE_NMAX = 3;
53 
54  int nper = static_cast<int>(M/(2.*M_PI));
55  M = M - nper*2.*M_PI;
56  if(M<0.) M = M + 2.*M_PI;
57  assert(M>=0.);
58  if(M>M_PI)
59  {
60  M = 2.*M_PI - M;
61  double x = pow(6.*M,1./3.);
62  for(int i=1;i<=ORBEL_EHIE_NMAX;++i)
63  x = improve_mean_to_eccentric_annomaly_guess(e,M,x);
64  x = 2.*M_PI-x;
65  return x;
66  }
67  else
68  {
69  double x = pow(6.*M,1./3.);
70  for(int i=1;i<=ORBEL_EHIE_NMAX;++i)
71  x = improve_mean_to_eccentric_annomaly_guess(e,M,x);
72  return x;
73  }
74 }
75 
76 void calc_cartesian_for_ellipse(double& x,double& y, double & z, double &vx, double &vy, double &vz, const double a, const double e, const double i, const double O, const double w, const double M, const double GM)
77 {
78  double cape = mean_to_eccentric_annomaly(e,M);
79  double scap, ccap;
80  sincos(cape,&scap,&ccap);
81  double sqe = sqrt(1.-e*e);
82  double sqgma = sqrt(GM*a);
83  double xfac1 = a*(ccap-e);
84  double xfac2 = a*sqe*scap;
85  double ri = 1./(a*(1.-e*ccap));
86  double vfac1 = -ri*sqgma * scap;
87  double vfac2 = ri*sqgma*sqe*ccap;
88 
89  double sw, cw, so, co, si, ci;
90  sincos(w,&sw,&cw);
91  sincos(O,&so,&co);
92  sincos(i,&si,&ci);
93  double d1[] = { cw*co-sw*so*ci, cw*so+sw*co*ci, sw*si};
94  double d2[] = {-sw*co-cw*so*ci,-sw*so+cw*co*ci, cw*si};
95  x = d1[0]*xfac1+d2[0]*xfac2;
96  y = d1[1]*xfac1+d2[1]*xfac2;
97  z = d1[2]*xfac1+d2[2]*xfac2;
98  vx = d1[0]*vfac1+d2[0]*vfac2;
99  vy = d1[1]*vfac1+d2[1]*vfac2;
100  vz = d1[2]*vfac1+d2[2]*vfac2;
101 }
102 void calc_keplerian_for_cartesian( double& a, double& e, double& i, double& O, double& w, double& M, const double x,const double y, const double z, const double vx, const double vy, const double vz, const double GM)
103 {
104  const double TINY = 1.e-8;
105 
106  double h[] = {y*vz-z*vy, z*vx-x*vz, x*vy-y*vx};
107  double h2 = h[0]*h[0]+h[1]*h[1]+h[2]*h[2];
108  double hh = sqrt(h2);
109  i = acos(h[2]/hh);
110  double fac = sqrt(h[0]*h[0]+h[1]*h[1])/hh;
111  double u;
112  if(fac<TINY)
113  {
114  O = 0.;
115  u = atan2(y,x);
116  if(fabs(i-M_PI)<10.*TINY) u = -u;
117  }
118  else
119  {
120  O = atan2(h[0],-h[1]);
121  u = atan2(z/sin(i),x*cos(O)+y*sin(O));
122  }
123  if(O<0.) O += 2.*M_PI;
124  if(u<0.) u += 2.*M_PI;
125  double r = sqrt(x*x+y*y+z*z);
126  double energy = (vx*vx+vy*vy+vz*vz)*0.5-GM/r;
127 
128  if(fabs(energy*r/GM)<sqrt(TINY))
129  { // Parabola
130  a = 0.5*h2/GM;
131  e = 1.;
132  double ww = acos(2.*a/r-1.);
133  if(vx*x+vy*y+vz*z<0.) w = 2.*M_PI-w;
134  double tmpf = tan(0.5*w);
135  M = tmpf*(1.+tmpf*tmpf/3.);
136  w = u-ww;
137  if(w<0.) w+= 2.*M_PI;
138  w -= round(w/(2.*M_PI))*2.*M_PI;
139  }
140  else if (energy<0)
141  { // Elipse
142  a = -0.5*GM/energy;
143  fac = 1.-h2/(GM*a);
144  double ww, cape;
145  if(fac>TINY)
146  {
147  e = sqrt(fac);
148  double face = (a-r)/(a*e);
149  if(face>1.) cape = 0.;
150  else if (face>-1.) cape = acos(face);
151  else cape = M_PI;
152 
153  if(vx*x+vy*y+vz*z<0.) cape = 2.*M_PI-cape;
154  double cw = (cos(cape)-e)/(1.-e*cos(cape));
155  double sw = sqrt(1.-e*e)*sin(cape)/(1.-e*cos(cape));
156  ww = atan2(sw,cw);
157  if(ww<0.) ww += 2.*M_PI;
158  }
159  else
160  {
161  e = 0.;
162  ww = u;
163  cape = u;
164  }
165  M = cape - e*sin(cape);
166  w = u - ww;
167  if(w<0.) w += 2.*M_PI;
168  w -= round(w/(2.*M_PI))*2.*M_PI;
169  }
170  else if (energy>0)
171  { // Hyperbola
172  a = 0.5*GM/energy;
173  fac = h2/(GM*a);
174  double ww, capf;
175  if(fac>TINY)
176  {
177  e = sqrt(1.+fac);
178  double tmpf = (a+r)/(a*e);
179  capf = log(tmpf+sqrt(tmpf*tmpf-1.));
180  if(vx*x+vy*y+vz*z<0.) capf = -capf;
181  double cw = (e-cosh(capf))/(e*cosh(capf)-1.);
182  double sw = sqrt(e*e-1.)*sinh(capf)/(e*cosh(capf)-1.);
183  ww = atan2(sw,cw);
184  if(ww<0.) ww += 2.*M_PI;
185  }
186  else
187  {
188  e = 1.;
189  double tmpf = 0.5*h2/GM;
190  ww = acos(2.*tmpf/r-1.);
191  if(vx*x+vy*y+vz*z<0.) ww = 2.*M_PI-ww;
192  tmpf = (a+r)/(a*e);
193  capf = log(tmpf+sqrt(tmpf*tmpf-1.));
194  }
195  M = e*sinh(capf)-capf;
196  w = u - ww;
197  if(w<0.) w+=2.*M_PI;
198  w -= round(w/(2.*M_PI))*2.*M_PI;
199  }
200 };
201