6D Cartesian and Spherical coordinates.

c6d_s6d.py

"""Conversion between 6D Cartesian and spherical coordinates.

Cartesian6D represents the coordinates (x, y, z, xdot, ydot, zdot).
Spherical6D represents the coordinates (r, alpha, delta, rdot,
alphadot, deltadot). Alpha is the longitudinal angle and delta is the
latitudinal angle. The latter goes from 90 degrees at the +ve z-axis to
-90 at the -ve z-axis.

The Cartesian coordinates can have any units. The radial spherical
coordinate has the some unit as the input Cartesian
coordinates. Angular quantities are radians and
radians/<appropriate-time-unit>.

The formulae used for Cartesian to spherical are:

  r = sqrt(x^2 + y^2 + z^2)
  alpha = arctan(y/x)  # longitude/azimuth
  delta = arcsin(z/r)  # latitude/elevation
  rdot = (x * xdot + y * ydot + z * zdot) / r
  alphadot = (x * xdot - y * ydot) / (r * cos(delta))**2
  deltadot = (zdot - (rdot * sin(delta))) / (r * cos(delta))

with the following conditions to deal with singularities:

+ If r=0, then rdot=xdot, and (alpha, delta, alphadot, deltadot) = 0.

+ If x=0, then alpha=(-pi/2, 0, pi/2) according to whether y is
  negative, zero or positive.

+ If cos(delta) = 0, then rdot = zdot / sin(delta), and either
  deltadot = -ydot / (r * sin(delta) * sin(alpha)) or
  deltadot = -xdot / (r * sin(delta) * cos(alpha)),
  depending on whether cos(alpha)=0.

The formuale for Spherical to Cartesian are:

x = r * cos(delta) * cos(alpha)
y = r * cos(delta) * sin(alpha)
x = r * sin(delta)

xdot = -r * (alphadot * cos(delta) * sin(alpha) + deltadot * sin(delta) * cos(alpha))
xdot += rdot * cos(delta) * cos(alpha)

ydot = r *  (alphadot * cos(delta) * cos(alpha) - deltadot * sin(delta) * sin(alpha))
ydot += rdot * cos(delta) * sin(delta)

zdot = r * deltadot * cos(delta) + rdot * sin(delta)


In both systems the velocities can be derived by taking derivaties of
the position coordinates.

The formulae and the defaults to avoid singularities, are taken from the
manual for Jeffrey W Percival's TPM C library: see
http://phn.github.com/pytpm/_downloads/tpm.pdf.

The formulae are also presented in Yallop et. al., 1989AJ.....97.1197K;
http://adsabs.harvard.edu/abs/1989AJ.....97.1197K .

I derived them too :) .

However, this Python code has not been tested.
"""
from math import sqrt, sin, cos, atan2
from math import pi

CLOSE_TO_ZERO = 1e-15  # The smallest is 2e-16?


class Cartesian6D(object):
    def __init__(self, x=0.0, y=0.0, z=0.0, xdot=0.0, ydot=0.0, zdot=0.0):
        self.x = x
        self.y = y
        self.z = z
        self.xdot = xdot
        self.ydot = ydot
        self.zdot = zdot

    @property
    def mod(self):
        return sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)

    def to_s(self):
        r = self.mod

        # Check for potential singularites i.e, division by 0.
        if r <= CLOSE_TO_ZERO:  # r == 0, r is always +ve
            rdot = self.xdot
            alpha, alphadot, delta, deltadot = 0.0, 0.0, 0.0, 0.0
            return Spherical6D(r=r, alpha=alpha, delta=delta,
                               rdot=rdot, alphadot=alphadot,
                               deltadot=deltadot)

        if (abs(self.x) <= CLOSE_TO_ZERO):
            if abs(self.y) <= CLOSE_TO_ZERO:
                alpha = 0.0
            elif self.y < 0:
                alpha = -pi / 2.0
            elif self.y > 0:
                alpha = pi / 2.0
        else:
            alpha = atan2(self.y, self.x)

        delta = atan2(self.z, sqrt(self.x ** 2 + self.y ** 2))

        if abs(cos(delta)) <= CLOSE_TO_ZERO:  # cos(delta) == 0; poles
            rdot = self.zdot / sin(delta)
            if abs(cos(alpha)) <= CLOSE_TO_ZERO:
                deltadot = -self.ydot / (r * sin(delta) * sin(alpha))
            else:
                deltadot = -self.xdot / (r * sin(delta) * cos(alpha))
            return Spherical6D(r=r, alpha=alpha, delta=delta,
                               rdot=rdot, alphadot=alphadot,
                               deltadot=deltadot)

        # If we are here then no singularities should occur.
        rdot = (self.x * self.xdot + self.y * self.ydot +
                self.z * self.zdot) / r
        alphadot = (self.x * self.ydot - self.y * self.xdot) / \
            (r * cos(delta)) ** 2
        deltadot = (self.zdot - rdot * sin(delta)) / (r * cos(delta))

        return Spherical6D(r=r, alpha=alpha, delta=delta,
                               rdot=rdot, alphadot=alphadot,
                               deltadot=deltadot)

    def __str__(self):
        s = "{0} {1} {2} {3} {4} {5}".format(
            self.x, self.y, self.z, self.xdot, self.ydot, self.zdot
            )
        return s


class Spherical6D(object):
    def __init__(self, r=0.0, alpha=0.0, delta=0.0, rdot=0.0,
                 alphadot=0.0, deltadot=0.0):
        self.r = r
        self.alpha = alpha
        self.delta = delta
        self.rdot = rdot
        self.alphadot = alphadot
        self.deltadot = deltadot

    def to_c(self):
        x = self.r * cos(self.delta) * cos(self.alpha)
        y = self.r * cos(self.delta) * sin(self.alpha)
        z = self.r * sin(self.delta)
        xdot = self.alphadot * cos(self.delta) * sin(self.alpha)
        xdot += self.deltadot * sin(self.delta) * cos(self.alpha)
        xdot *= -self.r
        xdot += self.rdot * cos(self.delta) * cos(self.alpha)

        ydot = self.alphadot * cos(self.delta) * cos(self.alpha)
        ydot -= self.deltadot * sin(self.delta) * sin(self.alpha)
        ydot *= self.r
        ydot += self.rdot * cos(self.delta) * sin(self.alpha)

        zdot = self.r * self.deltadot * cos(self.delta) + \
            self.rdot * sin(self.delta)

        return Cartesian6D(x, y, z, xdot, ydot, zdot)

    def __str__(self):
        s = "{0} {1} {2} {3} {4} {5}".format(
            self.r, self.alpha, self.delta, self.rdot, self.alphadot,
            self.deltadot
            )
        return s