TEuler GToE(matrix g)
{
double x,sf;
TEuler f;
x = g[2][2];
if(x*x<1)
sf = sqrt(1-x*x);
else
sf = 0.0;
if(sf>EPS){
f.a1 = -g[2][1]/sf;
f.a1 = acos2(f.a1);
if(g[2][0]<0) f.a1 = 2*PI-f.a1;
f.a = acos2(x);
f.a2 = g[1][2]/sf;
f.a2 = acos2(f.a2);
if(g[0][2]<0) f.a2 = 2*PI-f.a2;
}
else{
f.a1 = g[1][1];
f.a1 = acos2(f.a1);
if(g[0][1]<0) f.a1 = 2*PI-f.a1;
f.a = 0;
f.a2 = 0;
}
return(f);
}
struct reb_orbit reb_tools_particle_to_orbit_err(double G, struct reb_particle p, struct reb_particle primary, int* err){
struct reb_orbit o;
if (primary.m <= TINY){
*err = 1; // primary has no mass.
return reb_orbit_nan;
}
double mu,dx,dy,dz,dvx,dvy,dvz,vsquared,vcircsquared,vdiffsquared;
double hx,hy,hz,vr,rvr,muinv,ex,ey,ez,nx,ny,n,ea;
mu = G*(p.m+primary.m);
dx = p.x - primary.x;
dy = p.y - primary.y;
dz = p.z - primary.z;
dvx = p.vx - primary.vx;
dvy = p.vy - primary.vy;
dvz = p.vz - primary.vz;
o.r = sqrt ( dx*dx + dy*dy + dz*dz );
vsquared = dvx*dvx + dvy*dvy + dvz*dvz;
o.v = sqrt(vsquared);
vcircsquared = mu/o.r;
o.a = -mu/( vsquared - 2.*vcircsquared ); // semi major axis
hx = (dy*dvz - dz*dvy); //angular momentum vector
hy = (dz*dvx - dx*dvz);
hz = (dx*dvy - dy*dvx);
o.h = sqrt ( hx*hx + hy*hy + hz*hz ); // abs value of angular momentum
vdiffsquared = vsquared - vcircsquared;
if(o.r <= TINY){
*err = 2; // particle is on top of primary
return reb_orbit_nan;
}
vr = (dx*dvx + dy*dvy + dz*dvz)/o.r;
rvr = o.r*vr;
muinv = 1./mu;
ex = muinv*( vdiffsquared*dx - rvr*dvx );
ey = muinv*( vdiffsquared*dy - rvr*dvy );
ez = muinv*( vdiffsquared*dz - rvr*dvz );
o.e = sqrt( ex*ex + ey*ey + ez*ez ); // eccentricity
o.n = o.a/fabs(o.a)*sqrt(fabs(mu/(o.a*o.a*o.a))); // mean motion (negative if hyperbolic)
o.P = 2*M_PI/o.n; // period (negative if hyperbolic)
o.inc = acos2(hz, o.h, 1.); // cosi = dot product of h and z unit vectors. Always in [0,pi], so pass dummy disambiguator
// will = 0 if h is 0.
nx = -hy; // vector pointing along the ascending node = zhat cross h
ny = hx;
n = sqrt( nx*nx + ny*ny );
// Omega, pomega and theta are measured from x axis, so we can always use y component to disambiguate if in the range [0,pi] or [pi,2pi]
o.Omega = acos2(nx, n, ny); // cos Omega is dot product of x and n unit vectors. Will = 0 if i=0.
ea = acos2(1.-o.r/o.a, o.e, vr); // from definition of eccentric anomaly. If vr < 0, must be going from apo to peri, so ea = [pi, 2pi] so ea = -acos(cosea)
o.M = ea - o.e*sin(ea); // mean anomaly (Kepler's equation)
// in the near-planar case, the true longitude is always well defined for the position, and pomega for the pericenter if e!= 0
// we therefore calculate those and calculate the remaining angles from them
if(o.inc < MIN_INC || o.inc > M_PI - MIN_INC){ // nearly planar. Use longitudes rather than angles referenced to node for numerical stability.
o.pomega = acos2(ex, o.e, ey); // cos pomega is dot product of x and e unit vectors. Will = 0 if e=0.
o.theta = acos2(dx, o.r, dy); // cos theta is dot product of x and r vectors (true longitude). Will = 0 if e = 0.
if(o.inc < M_PI/2.){
o.omega = o.pomega - o.Omega;
o.f = o.theta - o.pomega;
o.l = o.pomega + o.M;
}
else{
o.omega = o.Omega - o.pomega;
o.f = o.pomega - o.theta;
o.l = o.pomega - o.M;
}
}
// in the non-planar case, we can't calculate the broken angles from vectors like above. omega+f is always well defined, and omega if e!=0
else{
double wpf = acos2(nx*dx + ny*dy, n*o.r, dz); // omega plus f. Both angles measured in orbital plane, and always well defined for i!=0.
o.omega = acos2(nx*ex + ny*ey, n*o.e, ez);
if(o.inc < M_PI/2.){
o.pomega = o.Omega + o.omega;
o.f = wpf - o.omega;
o.theta = o.Omega + wpf;
o.l = o.pomega + o.M;
}
else{
o.pomega = o.Omega - o.omega;
o.f = wpf - o.omega;
o.theta = o.Omega - wpf;
o.l = o.pomega - o.M;
}
}
return o;
}
