static void Main(string[] args)
{
try {
Geocentric earth = new Geocentric();
const double lat0 = 48 + 50/60.0, lon0 = 2 + 20/60.0; // Paris
LocalCartesian proj = new LocalCartesian(lat0, lon0, 0, earth);
{
// Sample forward calculation
double lat = 50.9, lon = 1.8, h = 0; // Calais
double x, y, z;
proj.Forward(lat, lon, h, out x, out y, out z);
Console.WriteLine(String.Format("{0} {1} {2}", x, y, z));
}
{
// Sample reverse calculation
double x = -38e3, y = 230e3, z = -4e3;
double lat, lon, h;
proj.Reverse(x, y, z, out lat, out lon, out h);
Console.WriteLine(String.Format("{0} {1} {2}", lat, lon, h));
}
}
catch (GeographicErr e) {
Console.WriteLine(String.Format("Caught exception: {0}", e.Message));
}
}
static void Main(string[] args)
{
try {
Geocentric earth = new Geocentric(Constants.WGS84.EquatorialRadius,
Constants.WGS84.Flattening);
// Alternatively: Geocentric earth = new Geocentric();
{
// Sample forward calculation
double lat = 27.99, lon = 86.93, h = 8820; // Mt Everest
double X, Y, Z;
earth.Forward(lat, lon, h, out X, out Y, out Z);
Console.WriteLine(String.Format("{0} {1} {2}",
Math.Floor(X / 1000 + 0.5),
Math.Floor(Y / 1000 + 0.5),
Math.Floor(Z / 1000 + 0.5)));
}
{
// Sample reverse calculation
double X = 302e3, Y = 5636e3, Z = 2980e3;
double lat, lon, h;
earth.Reverse(X, Y, Z, out lat, out lon, out h);
Console.WriteLine(String.Format("{0} {1} {2}", lat, lon, h));
}
}
catch (GeographicErr e) {
Console.WriteLine(String.Format("Caught exception: {0}", e.Message));
}
}
static void Main(string[] args)
{
try {
Geocentric earth = new Geocentric();
const double lat0 = 48 + 50 / 60.0, lon0 = 2 + 20 / 60.0; // Paris
LocalCartesian proj = new LocalCartesian(lat0, lon0, 0, earth);
{
// Sample forward calculation
double lat = 50.9, lon = 1.8, h = 0; // Calais
double x, y, z;
proj.Forward(lat, lon, h, out x, out y, out z);
Console.WriteLine(String.Format("{0} {1} {2}", x, y, z));
}
{
// Sample reverse calculation
double x = -38e3, y = 230e3, z = -4e3;
double lat, lon, h;
proj.Reverse(x, y, z, out lat, out lon, out h);
Console.WriteLine(String.Format("{0} {1} {2}", lat, lon, h));
}
}
catch (GeographicErr e) {
Console.WriteLine(String.Format("Caught exception: {0}", e.Message));
}
}
private void OnValidate(object sender, EventArgs e)
{
try
{
Geocentric g = new Geocentric();
string test = g.ToString();
g = new Geocentric(g.MajorRadius, g.Flattening);
double x, y, z, lat, lon, alt, tx, ty, tz;
double[,] rot;
g.Forward(32.0, -86.0, 45.0, out x, out y, out z, out rot);
g.Forward(32.0, -86.0, 45.0, out tx, out ty, out tz);
if (x != tx || y != ty || z != tz)
{
throw new Exception("Error in Forward");
}
g.Reverse(x, y, z, out lat, out lon, out alt, out rot);
g.Reverse(x, y, z, out tx, out ty, out tz);
if (lat != tx || lon != ty || alt != tz)
{
throw new Exception("Error in Reverse");
}
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Error detected", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
MessageBox.Show("No errors detected", "OK", MessageBoxButtons.OK, MessageBoxIcon.Information);
}
static void Main(string[] args)
{
try {
Geocentric earth = new Geocentric( Constants.WGS84.MajorRadius,
Constants.WGS84.Flattening);
// Alternatively: Geocentric earth = new Geocentric();
{
// Sample forward calculation
double lat = 27.99, lon = 86.93, h = 8820; // Mt Everest
double X, Y, Z;
earth.Forward(lat, lon, h, out X, out Y, out Z);
Console.WriteLine( String.Format( "{0} {1} {2}",
Math.Floor(X / 1000 + 0.5),
Math.Floor(Y / 1000 + 0.5),
Math.Floor(Z / 1000 + 0.5) ) );
}
{
// Sample reverse calculation
double X = 302e3, Y = 5636e3, Z = 2980e3;
double lat, lon, h;
earth.Reverse(X, Y, Z, out lat, out lon, out h);
Console.WriteLine(String.Format("{0} {1} {2}", lat, lon, h));
}
}
catch (GeographicErr e) {
Console.WriteLine( String.Format( "Caught exception: {0}", e.Message ) );
}
}
public void CartConvert1()
{
var cart = new Geocentric(6.4e6, -1.0 / 100);
var(lat, lon, h) = cart.Reverse(1e3, 0, 10e3);
Assert.AreEqual(4.42, lat, 0.01);
Assert.AreEqual(0.00, lon, 0.001);
Assert.AreEqual(-6398614, h, 1);
}
public void CartConvert0()
{
var cart = new Geocentric(6.4e6, 1.0 / 100);
var(lat, lon, h) = cart.Reverse(10e3, 0, 1e3);
Assert.AreEqual(85.57, lat, 0.01);
Assert.AreEqual(0.00, lon, 0.001);
Assert.AreEqual(-6334614, h, 1);
}
public LocalCartesianPanel()
{
InitializeComponent();
Geocentric g = new Geocentric();
m_majorRadiusTextBox.Text = g.MajorRadius.ToString();
m_flatteningTextBox.Text = g.Flattening.ToString();
m_lc = new LocalCartesian(g);
m_functionComboBox.SelectedIndex = (int)m_function;
}
public LocalCartesianPanel()
{
InitializeComponent();
Geocentric g = new Geocentric();
m_majorRadiusTextBox.Text = g.EquatorialRadius.ToString();
m_flatteningTextBox.Text = g.Flattening.ToString();
m_lc = new LocalCartesian(g);
m_functionComboBox.SelectedIndex = (int)m_function;
}
private void OnSetParameters(object sender, EventArgs e)
{
try
{
double a = Double.Parse(m_majorRadiusTextBox.Text);
double f = Double.Parse(m_flatteningTextBox.Text);
m_geocentric = new Geocentric(a, f);
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Data entry error", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
}
public GeocentricPanel()
{
InitializeComponent();
try
{
m_geocentric = new Geocentric();
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
m_majorRadiusTextBox.Text = m_geocentric.MajorRadius.ToString();
m_flatteningTextBox.Text = m_geocentric.Flattening.ToString();
m_functionComboBox.SelectedIndex = (int)m_function;
}
public GeocentricPanel()
{
InitializeComponent();
try
{
m_geocentric = new Geocentric();
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
m_majorRadiusTextBox.Text = m_geocentric.MajorRadius.ToString();
m_flatteningTextBox.Text = m_geocentric.Flattening.ToString();
m_functionComboBox.SelectedIndex = (int)m_function;
}
private void OnValidate(object sender, EventArgs e)
{
try
{
double lon = -86.0;
double lat = 32.0;
double alt = 0.0;
double x, y, z;
GravityModel gm = new GravityModel(m_name, m_path);
gm.Disturbance(lat, lon, alt, out x, out y, out z);
gm.GeoidHeight(lat, lon);
gm.Gravity(lat, lon, alt, out x, out y, out z);
gm.Phi(5000000.0, 5000000.0, out x, out y);
gm.SphericalAnomaly(lat, lon, alt, out x, out y, out z);
gm.T(5000000.0, 5000000.0, 5000000.0);
gm.U(5000000.0, 5000000.0, 5000000.0, out x, out y, out z);
gm.V(5000000.0, 5000000.0, 5000000.0, out x, out y, out z);
gm.W(5000000.0, 5000000.0, 5000000.0, out x, out y, out z);
NormalGravity ng = new NormalGravity(NormalGravity.StandardModels.GRS80);
ng = new NormalGravity(NormalGravity.StandardModels.WGS84);
ng = new NormalGravity(6378137.0, 3.986005e+14, 7.292115147e-5, 1.08263e-3, false);
ng = gm.ReferenceEllipsoid();
ng.DynamicalFormFactor(1);
Geocentric geo = ng.Earth();
ng.Gravity(lat, alt, out x, out z);
ng.Phi(5000000.0, 5000000.0, out x, out y);
ng.SurfaceGravity(lat);
ng.U(5000000.0, 5000000.0, 5000000.0, out x, out y, out z);
ng.V0(5000000.0, 5000000.0, 5000000.0, out x, out y, out z);
GravityCircle gc = gm.Circle(lat, 0.0, GravityModel.Mask.ALL);
gc.Capabilities();
gc.Capabilities(GravityModel.Mask.GRAVITY);
gc.Disturbance(lon, out x, out y, out z);
gc.GeoidHeight(lon);
gc.Gravity(lon, out x, out y, out z);
gc.SphericalAnomaly(lon, out x, out y, out z);
gc.T(lon);
gc.V(lon, out x, out y, out z);
gc.W(lon, out x, out y, out z);
MessageBox.Show("No errors detected", "OK", MessageBoxButtons.OK, MessageBoxIcon.Information);
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
public static DateTimeOffset VernalEquinoxTimeOf(int year)
{
if (Vernals == null)
{
Vernals = new Dictionary <int, DateTimeOffset>();
}
if (!Vernals.ContainsKey(year))
{
double jul_ut = Utilities.ToJulianDay(new DateTimeOffset(year, 3, 15, 12, 0, 0, TimeSpan.Zero));
double averageSpeed = 360.0 / AverageYearLength;
Position posDynamic = Geocentric.PositionDelegate(jul_ut, PlanetId.SE_SUN);
double shiftDynamic;
double distanceDynamic = Angle.BeelineOf(posDynamic.Longitude, 360);
shiftDynamic = distanceDynamic / averageSpeed;
for (int i = 0; i < 10; i++)
{
posDynamic = Geocentric.PositionDelegate(jul_ut + shiftDynamic, PlanetId.SE_SUN);
distanceDynamic = Angle.BeelineOf(posDynamic.Longitude, 360);
if (Math.Abs(distanceDynamic) < Negligible)
{
break;
}
jul_ut += shiftDynamic;
shiftDynamic = distanceDynamic / posDynamic.LongitudeVelocity;
}
Vernals.Add(year, Utilities.UtcFromJulianDay(jul_ut));
}
return(Vernals[year]);
}
private void OnValidate(object sender, EventArgs e)
{
try
{
Geocentric g = new Geocentric();
string test = g.ToString();
g = new Geocentric(g.MajorRadius, g.Flattening);
double x, y, z, lat, lon, alt, tx, ty, tz;
double[,] rot;
g.Forward(32.0, -86.0, 45.0, out x, out y, out z, out rot);
g.Forward(32.0, -86.0, 45.0, out tx, out ty, out tz);
if (x != tx || y != ty || z != tz)
throw new Exception("Error in Forward");
g.Reverse(x, y, z, out lat, out lon, out alt, out rot);
g.Reverse(x, y, z, out tx, out ty, out tz);
if ( lat != tx || lon != ty || alt != tz )
throw new Exception("Error in Reverse");
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Error detected", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
MessageBox.Show("No errors detected", "OK", MessageBoxButtons.OK, MessageBoxIcon.Information);
}
private void OnSetParameters(object sender, EventArgs e)
{
try
{
double a = Double.Parse( m_majorRadiusTextBox.Text );
double f = Double.Parse(m_flatteningTextBox.Text);
m_geocentric = new Geocentric(a, f);
}
catch (Exception xcpt)
{
MessageBox.Show(xcpt.Message, "Data entry error", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
}
void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
{
packetCount++;
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
//OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);
if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts)
{
lastPacketTime = new CarTimestamp(-10000);
}
if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02)
{
DatasetSource ds = Services.Dataset;
PoseAbsData d = e.PoseAbsData;
CarTimestamp now = e.PoseAbsData.timestamp;
if (Services.Projection != null)
{
Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <Coordinates>("xy").Add(xy, now);
AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
if (!Settings.UseWheelSpeed)
{
ds.ItemAs <double>("speed").Add(d.veh_vx, now);
}
ds.ItemAs <double>("vel - y").Add(d.veh_vy, now);
ds.ItemAs <double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
ds.ItemAs <double>("pitch").Add(d.pitch + 1.25 * Math.PI / 180.0, now);
ds.ItemAs <double>("roll").Add(d.roll, now);
ds.ItemAs <double>("ba - x").Add(d.bax, now);
ds.ItemAs <double>("ba - y").Add(d.bay, now);
ds.ItemAs <double>("ba - z").Add(d.baz, now);
ds.ItemAs <double>("bw - x").Add(d.bwx, now);
ds.ItemAs <double>("bw - y").Add(d.bwy, now);
ds.ItemAs <double>("bw - z").Add(d.bwz, now);
ds.ItemAs <PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);
LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <double>("altitude").Add(lla.alt, now);
if (Services.Projection != null)
{
ds.ItemAs <Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
ds.ItemAs <Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
}
ds.ItemAs <double>("sep heading").Add(d.sep_heading, now);
ds.ItemAs <double>("sep pitch").Add(d.sep_pitch, now);
ds.ItemAs <double>("sep roll").Add(d.sep_roll, now);
if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0)
{
// get the enu velocity
Vector3 pECEF = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
Vector3 vECEF = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
Vector3 vENU = Recef2enu * vECEF;
BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts - lastPacketTime.ts, now);
Services.Dataset.ItemAs <double>("heading bias").Add(BiasEstimator.HeadingBias, now);
}
lastPacketTime = now;
}
}
