public Lla ToLla()
{
Lla lla = new Lla();
double radius_p = 0;
double dist_to_z = Math.Sqrt(X * X + Y * Y);
lla.Lat = Math.Atan2(Z, (1 - GPS.EECC_SQUARED) * dist_to_z);
for (int i = 1; i <= 5; ++i)
{
double sin_lat = Math.Sin(lla.Lat);
radius_p = GPS.ESMAJ / Math.Sqrt(1.0 - GPS.EECC_SQUARED * sin_lat * sin_lat);
lla.Lat = Math.Atan2(Z + GPS.EECC_SQUARED * radius_p * sin_lat, dist_to_z);
}
lla.Lon = Math.Atan2(Y, X);
if (lla.LatDeg < -85 || lla.LatDeg > 85) // If we are close to the poles
{
double L = Z + GPS.EECC_SQUARED * radius_p * Math.Sin(lla.Lat);
lla.Alt = L / Math.Sin(lla.Lat) - radius_p;
}
else
{
lla.Alt = dist_to_z / Math.Cos(lla.Lat) - radius_p;
}
return(lla);
}
public Ecef(Lla lla)
: this()
{
Ecef ecef = lla.ToEcef();
X = ecef.X;
Y = ecef.Y;
Z = ecef.Z;
}
public Enu ToEnu(Lla origin)
{
Enu enu = new Enu();
enu.East = (Lon - origin.Lon) * GPS.ESMAJ * Math.Cos(Lat);
enu.North = (Lat - origin.Lat) * GPS.ESMAJ;
enu.Up = Alt - origin.Alt;
return(enu);
}
public Tuple<Ecef, Ecef> GeneratePositionAndVelocityAt(double elapsedTime)
{
double time = elapsedTime / 1000.0;
double posOnCircle = time * this.SPEED / RADIUS;
double e = Math.Cos(posOnCircle) * RADIUS;
double n = Math.Sin(posOnCircle) * RADIUS;
Lla llaPos = ORIGIN.AddEnu(new Enu(e, n, 0));
Ecef position = llaPos.ToEcef();
return new Tuple<Ecef, Ecef>(position, ComputeVelocity(posOnCircle));
}
public Ecef ToEcef(Lla origin)
{
Ecef originEcef = origin.ToEcef();
double sinLon = Math.Sin(origin.Lon);
double cosLon = Math.Cos(origin.Lon);
double sinLat = Math.Sin(origin.Lat);
double cosLat = Math.Cos(origin.Lat);
return(new Ecef(
-sinLon * East - sinLat * cosLon * North + cosLat * cosLon * Up + originEcef.X,
cosLon * East - sinLat * sinLon * North + cosLat * sinLon * Up + originEcef.Y,
cosLat * North + sinLat * Up + originEcef.Z
));
}
public bool PushLlaNed(double elapsedTime, Lla lla, Attitude attitude, string name = "")
{
return(PushEcefNed(elapsedTime, lla.ToEcef(), attitude, name));
}
public bool PushLla(double elapsedTime, Lla lla, string name = "")
{
return(PushEcef(elapsedTime, lla.ToEcef(), name));
}
public void PushRouteLla(double speed, Lla lla)
{
PushRouteEcef(speed, lla.ToEcef());
}
public void PushTrackLlaNed(int elapsedTime, Lla lla, Attitude attitude)
{
PushTrackEcefNed(elapsedTime, lla.ToEcef(), attitude);
}
public void PushTrackLla(int elapsedTime, Lla lla)
{
PushTrackEcef(elapsedTime, lla.ToEcef());
}
public Lla ToLla(Lla origin)
{
return(ToEcef(origin).ToLla());
}