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 Cartesians operator -(Cartesians a, Cartesians b)
{
var positionX = a.PositionX - b.PositionX;
var positionY = a.PositionY - b.PositionY;
var positionZ = a.PositionZ - b.PositionZ;
var velocityX = a.VelocityX - b.VelocityX;
var velocityY = a.VelocityY - b.VelocityY;
var velocityZ = a.VelocityZ - b.VelocityZ;
var result = new Cartesians(positionX, positionY, positionZ, velocityX, velocityY, velocityZ);
return(result);
}
public static Matrix JacobianPolarsWrtCartesians(Cartesians cartesians)
{
var polars = CartesiansToPolars(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 = polars.Range;
var rDot = polars.RangeRate;
var phiDot = polars.AzimuthRate;
var theta = polars.ElevationAngle;
var thetaDot = polars.AzimuthRate;
var rg = r * Math.Cos(theta);
var rSquared = r * r;
var rgSquared = rg * rg;
var j = new double[6, 6];
j[0, 0] = x / r;
j[0, 1] = y / r;
j[0, 2] = z / r;
j[0, 3] = 0.0;
j[0, 4] = 0.0;
j[0, 5] = 0.0;
j[1, 0] = (r * vx - rDot * x) / rSquared;
j[1, 1] = (r * vy - rDot * y) / rSquared;
j[1, 2] = (r * vz - rDot * z) / rSquared;
j[1, 3] = x / r;
j[1, 4] = y / r;
j[1, 5] = z / r;
j[2, 0] = -y / rgSquared;
j[2, 1] = x / rgSquared;
j[2, 2] = 0.0;
j[2, 3] = 0.0;
j[2, 4] = 0.0;
j[2, 5] = 0.0;
j[3, 0] = (vy - 2 * x * phiDot) / rgSquared;
j[3, 1] = (-vx - 2 * y * phiDot) / rgSquared;
j[3, 2] = 0.0;
j[3, 3] = -y / rgSquared;
j[3, 4] = x / rgSquared;
j[3, 5] = 0.0;
j[4, 0] = (x * z) / (rg * rSquared);
j[4, 1] = (y * z) / (rg * rSquared);
j[4, 2] = -rg / rSquared;
j[4, 3] = 0.0;
j[4, 4] = 0.0;
j[4, 5] = 0.0;
j[5, 0] = (j[1, 0] - (rDot * j[0, 0] / r)) * z / (r * rg) - thetaDot * x / rgSquared;
j[5, 1] = (j[1, 1] - (rDot * j[0, 1] / r)) * z / (r * rg) - thetaDot * y / rgSquared;
j[5, 2] = (j[1, 2] - (rDot * j[0, 2] / r)) * z / (r * rg) + rDot / (r * rg);
j[5, 3] = j[4, 0];
j[5, 4] = j[4, 1];
j[5, 5] = j[4, 2];
var result = new Matrix(j);
return(result);
}