/// <summary>
/// Computes the cross product of this <see cref="vec3d"/> and the
/// provided <see cref="vec3d"/>.
/// </summary>
/// <param name="rhs">
/// The <see cref="vec3d"/> to compute the cross product with this.
/// </param>
/// <returns>
/// The computed cross product.
/// </returns>
public vec3d CrossProduct(vec3d rhs)
{
return(new vec3d(
Y * rhs.Z - Z * rhs.Y,
Z * rhs.X - X * rhs.Z,
X * rhs.Y - Y * rhs.X));
}
/// <summary>
/// Subtracts the provided <see cref="vec3d"/> from this
/// <see cref="vec3d"/>.
/// </summary>
/// <param name="rhs">
/// The <see cref="vec3d"/> to subtract from this
/// <see cref="vec3d"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec3d"/> from the subtraction.
/// </returns>
public vec3d Subtract(vec3d rhs)
{
return(new vec3d(
X - rhs.X,
Y - rhs.Y,
Z - rhs.Z));
}
/// <summary>
/// Adds this <see cref="vec3d"/> with the provided
/// <see cref="vec3d"/> together.
/// </summary>
/// <param name="vector">
/// The <see cref="vec3d"/> to add to this <see cref="vec3d"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec3d"/> from the addition.
/// </returns>
public vec3d Add(vec3d vector)
{
return(new vec3d(
X + vector.X,
Y + vector.Y,
Z + vector.Z));
}
/// <summary>
/// Converts ECEF coordinate to LLA frame.
/// </summary>
/// <returns>Coordinate converted to LLA frame.</returns>
/// <param name="ecef">Coordinate in ECEF frame.</param>
public static vec3d Ecef2Lla(vec3d ecef)
{
double x, y, z, r, c_phi, c_phi0, s_phi,
s_phi0, tau, lat, lon, eta, h;
const double Rthresh = 0.001; /* Limit on distance from pole in km to switch calculation. */
x = ecef.X;
y = ecef.Y;
z = ecef.Z;
r = System.Math.Sqrt(x * x + y * y);
if (r < Rthresh)
{
c_phi0 = 0;
s_phi0 = System.Math.Sign(z);
}
else
{
double tau0 = z / (EPSILON * r);
c_phi0 = 1 / System.Math.Sqrt(1 + tau0 * tau0);
s_phi0 = tau0 * c_phi0;
}
tau = (z + BBAR * s_phi0 * s_phi0 * s_phi0) / (r - ABAR * c_phi0 * c_phi0 * c_phi0);
lat = System.Math.Atan(tau);
if (r < Rthresh)
{
c_phi = 0;
s_phi = System.Math.Sign(z);
}
else
{
c_phi = 1 / System.Math.Sqrt(1 + tau * tau);
s_phi = tau * c_phi;
}
eta = System.Math.Sqrt(1 - E2 * s_phi * s_phi);
h = r * c_phi + z * s_phi - A * eta;
lon = System.Math.Atan2(y, x);
return(new vec3d(
lat * 180 / Const.PI,
lon * 180 / Const.PI,
h * 1000
));
}
/// <summary>
/// Converts LLA coordinate to ECEF frame.
/// </summary>
/// <returns>Coordinate converted to ECEF frame in (km, km, km) units.</returns>
/// <param name="lla">Coordinate in LLA frame in (deg, deg, meter) units.</param>
public static vec3d Lla2Ecef(vec3d lla)
{
double lat, lon, alt;
double n, x, y, z;
double t1; /* TEMPS */
lat = lla.X * Const.PI / 180; /* Geodetic latitude in radians. */
lon = lla.Y * Const.PI / 180; /* Longitude in radians. */
alt = lla.Z / 1000; /* Altitude above WGS84 in km. */
t1 = System.Math.Sin(lat);
n = A / System.Math.Sqrt(1 - E2 * t1 * t1);
t1 = alt + n;
x = t1 * System.Math.Cos(lat) * System.Math.Cos(lon);
y = t1 * System.Math.Cos(lat) * System.Math.Sin(lon);
z = (t1 - E2 * n) * System.Math.Sin(lat);
return(new vec3d(x, y, z));
}
/// <summary>
/// Creates a new <c>PositionD</c> from a latitude, longitude, altitude.
/// </summary>
/// <param name="lla">
/// The position expressed as a latitude, longitude, altitude.
/// </param>
/// <returns>
/// The new <c>PositionD</c>.
/// </returns>
public static PositionD FromLla(vec3d lla)
{
return(new PositionD(PositionType.Lla, lla));
}
/// <summary>
/// Creates a new <c>PositionD</c> from an earth-centered, earth-fixed.
/// </summary>
/// <param name="ecef">
/// The position expressed as an earth-centered, earth-fixed.
/// </param>
/// <returns>
/// The new <c>PositionD</c>.
/// </returns>
public static PositionD FromEcef(vec3d ecef)
{
return(new PositionD(PositionType.Ecef, ecef));
}
/// <summary>
/// Computes the dot product of this <see cref="vec3d"/> and the
/// provided <see cref="vec3d"/>.
/// </summary>
/// <param name="vector">
/// The <see cref="vec3d"/> to compute the dot product with this.
/// </param>
/// <returns>
/// The computed dot product.
/// </returns>
public double DotProduct(vec3d vector)
{
return(X * vector.X + Y * vector.Y + Z * vector.Z);
}
