public static Polars CartesiansToPolars(Cartesians cartesians)
{
var x = cartesians.PositionX;
var y = cartesians.PositionY;
var z = cartesians.PositionZ;
var vx = cartesians.VelocityX;
var vy = cartesians.VelocityY;
var vz = cartesians.VelocityZ;
var r = Math.Sqrt(x * x + y * y + z * z);
var phi = Math.Atan2(y, x);
var theta = Math.Asin(-z / r);
var cp = Math.Cos(phi);
var sp = Math.Sin(phi);
var ct = Math.Cos(theta);
var st = Math.Sin(theta);
var rDot = (x * vx + y * vy + z * vz) / r;
var phiDot = (vy * cp - vx * sp) / (r * ct);
var thetaDot = -(vz * ct + (vx * cp + vy * sp) * st) / r;
var polars = new Polars(r, rDot, phi, phiDot, theta, thetaDot);
return(polars);
}
public static Cartesians PolarsToCartesians(Polars polars)
{
var r = polars.Range;
var rDot = polars.RangeRate;
var phi = polars.AzimuthAngle;
var phiDot = polars.AzimuthRate;
var theta = polars.ElevationAngle;
var thetaDot = polars.ElevationRate;
var cp = Math.Cos(phi);
var sp = Math.Sin(phi);
var ct = Math.Cos(theta);
var st = Math.Sin(theta);
var x = r * cp * ct;
var y = r * sp * ct;
var z = -r * st;
var vx = rDot * cp * ct - r * sp * ct * phiDot - r * cp * st * thetaDot;
var vy = rDot * sp * ct + r * cp * ct * phiDot - r * sp * st * thetaDot;
var vz = -rDot * st - r * ct * thetaDot;
var cartesians = new Cartesians(x, y, z, vx, vy, vz);
return(cartesians);
}
public static Matrix JacobianCartesiansWrtPolars(Polars polars)
{
var r = polars.Range;
var rDot = polars.RangeRate;
var phi = polars.AzimuthAngle;
var phiDot = polars.AzimuthRate;
var theta = polars.ElevationAngle;
var thetaDot = polars.AzimuthRate;
var cp = Math.Cos(phi);
var sp = Math.Sin(phi);
var ct = Math.Cos(theta);
var st = Math.Sin(theta);
var j = new double[6, 6];
j[0, 0] = cp * ct;
j[0, 1] = 0;
j[0, 2] = -r * sp * ct;
j[0, 3] = 0;
j[0, 4] = -r * cp * st;
j[0, 5] = 0;
j[1, 0] = sp * ct;
j[1, 1] = 0;
j[1, 2] = r * cp * ct;
j[1, 3] = 0;
j[1, 4] = -r * sp * st;
j[1, 5] = 0;
j[2, 0] = -st;
j[2, 1] = 0;
j[2, 2] = 0;
j[2, 3] = 0;
j[2, 4] = -r * ct;
j[2, 5] = 0;
j[3, 0] = -sp * ct * phiDot - cp * st * thetaDot;
j[3, 1] = cp * ct;
j[3, 2] = -rDot * sp * ct - r * cp * ct * phiDot + r * sp * st * thetaDot;
j[3, 3] = -r * sp * ct;
j[3, 4] = -rDot * cp * st + r * sp * st * phiDot - r * cp * ct * thetaDot;
j[3, 5] = -r * cp * st;
j[4, 0] = +cp * ct * phiDot - sp * st * thetaDot;
j[4, 1] = sp * ct;
j[4, 2] = rDot * cp * ct - r * sp * ct * phiDot - r * cp * st * thetaDot;
j[4, 3] = +r * cp * ct;
j[4, 4] = -rDot * sp * st - r * cp * st * phiDot - r * sp * ct * thetaDot;
j[4, 5] = -r * sp * st;
j[5, 0] = -ct * thetaDot;
j[5, 1] = -st;
j[5, 2] = 0;
j[5, 3] = 0;
j[5, 4] = -rDot * ct + r * st * thetaDot;
j[5, 5] = -r * ct;
var result = new Matrix(j);
return(result);
}