Swarm-NG  1.1
kepler.h
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 
19 
27 #pragma once
28 
29 double improve_mean_to_eccentric_annomaly_guess(const double e, const double M, const double x);
30 double mean_to_eccentric_annomaly(const double e, double M);
31 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);
32 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);
33 
34 // Code in this file largely adapted from Mercury.
35 
36 double improve_mean_to_eccentric_annomaly_guess(const double e, const double M, const double x)
37 {
38  // Based on Mercury
39  // double sx, cx;
40  // sincos(x,&sx,&cx);
41  double sx = sin(x);
42  double cx = cos(x);
43  double es = e*sx;
44  double ec = e*cx;
45  double f = x-es-M;
46  double fp = 1.-ec;
47  double fpp = es;
48  double fppp = ec;
49  double dx = -f/fp;
50  dx = -f/(fp+dx*fpp/2.);
51  dx = -f/(fp+dx*fpp/2.+dx*dx*fppp/6.);
52  return x+dx;
53 };
54 
56 double mean_to_eccentric_annomaly(const double e, double M)
57 {
58  // Based on Mercury
59  const int ORBEL_EHIE_NMAX = 3;
60 
61  int nper = static_cast<int>(M/(2.*M_PI));
62  M = M - nper*2.*M_PI;
63  if(M<0.) M = M + 2.*M_PI;
64  assert(M>=0.);
65  if(M>M_PI)
66  {
67  M = 2.*M_PI - M;
68  double x = pow(6.*M,1./3.);
69  for(int i=1;i<=ORBEL_EHIE_NMAX;++i)
70  x = improve_mean_to_eccentric_annomaly_guess(e,M,x);
71  x = 2.*M_PI-x;
72  return x;
73  }
74  else
75  {
76  double x = pow(6.*M,1./3.);
77  for(int i=1;i<=ORBEL_EHIE_NMAX;++i)
78  x = improve_mean_to_eccentric_annomaly_guess(e,M,x);
79  return x;
80  }
81 }
82 
84 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)
85 {
86  double cape = mean_to_eccentric_annomaly(e,M);
87  double scap, ccap;
88  sincos(cape,&scap,&ccap);
89  double sqe = sqrt(1.-e*e);
90  double sqgma = sqrt(GM*a);
91  double xfac1 = a*(ccap-e);
92  double xfac2 = a*sqe*scap;
93  double ri = 1./(a*(1.-e*ccap));
94  double vfac1 = -ri*sqgma * scap;
95  double vfac2 = ri*sqgma*sqe*ccap;
96 
97  double sw, cw, so, co, si, ci;
98  sincos(w,&sw,&cw);
99  sincos(O,&so,&co);
100  sincos(i,&si,&ci);
101  double d1[] = { cw*co-sw*so*ci, cw*so+sw*co*ci, sw*si};
102  double d2[] = {-sw*co-cw*so*ci,-sw*so+cw*co*ci, cw*si};
103  x = d1[0]*xfac1+d2[0]*xfac2;
104  y = d1[1]*xfac1+d2[1]*xfac2;
105  z = d1[2]*xfac1+d2[2]*xfac2;
106  vx = d1[0]*vfac1+d2[0]*vfac2;
107  vy = d1[1]*vfac1+d2[1]*vfac2;
108  vz = d1[2]*vfac1+d2[2]*vfac2;
109 }
110 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)
111 {
112  const double TINY = 1.e-8;
113 
114  double h[] = {y*vz-z*vy, z*vx-x*vz, x*vy-y*vx};
115  double h2 = h[0]*h[0]+h[1]*h[1]+h[2]*h[2];
116  double hh = sqrt(h2);
117  i = acos(h[2]/hh);
118  double fac = sqrt(h[0]*h[0]+h[1]*h[1])/hh;
119  double u;
120  if(fac<TINY)
121  {
122  O = 0.;
123  u = atan2(y,x);
124  if(fabs(i-M_PI)<10.*TINY) u = -u;
125  }
126  else
127  {
128  O = atan2(h[0],-h[1]);
129  u = atan2(z/sin(i),x*cos(O)+y*sin(O));
130  }
131  if(O<0.) O += 2.*M_PI;
132  if(u<0.) u += 2.*M_PI;
133  double r = sqrt(x*x+y*y+z*z);
134  double energy = (vx*vx+vy*vy+vz*vz)*0.5-GM/r;
135 
136  if(fabs(energy*r/GM)<sqrt(TINY))
137  { // Parabola
138  a = 0.5*h2/GM;
139  e = 1.;
140  double ww = acos(2.*a/r-1.);
141  if(vx*x+vy*y+vz*z<0.) w = 2.*M_PI-w;
142  double tmpf = tan(0.5*w);
143  M = tmpf*(1.+tmpf*tmpf/3.);
144  w = u-ww;
145  if(w<0.) w+= 2.*M_PI;
146  w -= round(w/(2.*M_PI))*2.*M_PI;
147  }
148  else if (energy<0)
149  { // Elipse
150  a = -0.5*GM/energy;
151  fac = 1.-h2/(GM*a);
152  double ww, cape;
153  if(fac>TINY)
154  {
155  e = sqrt(fac);
156  double face = (a-r)/(a*e);
157  if(face>1.) cape = 0.;
158  else if (face>-1.) cape = acos(face);
159  else cape = M_PI;
160 
161  if(vx*x+vy*y+vz*z<0.) cape = 2.*M_PI-cape;
162  double cw = (cos(cape)-e)/(1.-e*cos(cape));
163  double sw = sqrt(1.-e*e)*sin(cape)/(1.-e*cos(cape));
164  ww = atan2(sw,cw);
165  if(ww<0.) ww += 2.*M_PI;
166  }
167  else
168  {
169  e = 0.;
170  ww = u;
171  cape = u;
172  }
173  M = cape - e*sin(cape);
174  w = u - ww;
175  if(w<0.) w += 2.*M_PI;
176  w -= round(w/(2.*M_PI))*2.*M_PI;
177  }
178  else if (energy>0)
179  { // Hyperbola
180  a = 0.5*GM/energy;
181  fac = h2/(GM*a);
182  double ww, capf;
183  if(fac>TINY)
184  {
185  e = sqrt(1.+fac);
186  double tmpf = (a+r)/(a*e);
187  capf = log(tmpf+sqrt(tmpf*tmpf-1.));
188  if(vx*x+vy*y+vz*z<0.) capf = -capf;
189  double cw = (e-cosh(capf))/(e*cosh(capf)-1.);
190  double sw = sqrt(e*e-1.)*sinh(capf)/(e*cosh(capf)-1.);
191  ww = atan2(sw,cw);
192  if(ww<0.) ww += 2.*M_PI;
193  }
194  else
195  {
196  e = 1.;
197  double tmpf = 0.5*h2/GM;
198  ww = acos(2.*tmpf/r-1.);
199  if(vx*x+vy*y+vz*z<0.) ww = 2.*M_PI-ww;
200  tmpf = (a+r)/(a*e);
201  capf = log(tmpf+sqrt(tmpf*tmpf-1.));
202  }
203  M = e*sinh(capf)-capf;
204  w = u - ww;
205  if(w<0.) w+=2.*M_PI;
206  w -= round(w/(2.*M_PI))*2.*M_PI;
207  }
208 };
209 
210