public static double getDistance(XYZCoord a, XYZCoord b)
{
double d = Math.Pow(a.X - b.X, 2.0) + Math.Pow(a.Y - b.Y, 2.0);
d += Math.Pow(a.Z - b.Z, 2.0);
return(Math.Sqrt(d));
}
private XYZCoord LinearInterpolate(XYZCoord in1, XYZCoord in2, double percent)
{
XYZCoord coord;
GPSUtilsClass class2 = new GPSUtilsClass();
double[] numArray = new double[] { in1.X, in1.Y, in1.Z };
double[] numArray2 = new double[] { in2.X, in2.Y, in2.Z };
double[] numArray3 = new double[3];
numArray3 = class2.LinearInterpolateXYZ(numArray, numArray2, percent);
coord.X = numArray3[0];
coord.Y = numArray3[1];
coord.Z = numArray3[2];
return(coord);
}
public bool GetPositionAtTime(double inTime, out XYZCoord result)
{
result.X = 0.0;
result.Y = 0.0;
result.Z = 0.0;
try
{
int num = this.m_PositionData.IndexOfKey(inTime);
if (num != -1)
{
result = this.m_PositionData.Values[num];
this.m_current_idx = num;
return(true);
}
int num2 = 0;
int num3 = this.m_PositionData.Count - 1;
return(this.GetPositionAtTimeBetweenIndexes(inTime, this.m_current_idx, num3, out result) || this.GetPositionAtTimeBetweenIndexes(inTime, num2, num3, out result));
}
catch
{
}
return(false);
}
public bool GetPositionAtTime(double inTime, out XYZCoord result)
{
result.X = 0.0;
result.Y = 0.0;
result.Z = 0.0;
try
{
int num = this.m_PositionData.IndexOfKey(inTime);
if (num != -1)
{
result = this.m_PositionData.Values[num];
this.m_current_idx = num;
return true;
}
int num2 = 0;
int num3 = this.m_PositionData.Count - 1;
return (this.GetPositionAtTimeBetweenIndexes(inTime, this.m_current_idx, num3, out result) || this.GetPositionAtTimeBetweenIndexes(inTime, num2, num3, out result));
}
catch
{
}
return false;
}
private bool GetPositionAtTimeBetweenIndexes(double inTime, int low_idx, int high_idx, out XYZCoord result)
{
try
{
if ((inTime >= this.m_PositionData.Keys[low_idx]) && (inTime <= this.m_PositionData.Keys[high_idx]))
{
int num = low_idx;
while (num < high_idx)
{
if (inTime <= this.m_PositionData.Keys[num])
{
break;
}
num++;
}
num--;
this.m_current_idx = num;
double percent = (100.0 * (inTime - this.m_PositionData.Keys[num])) / (this.m_PositionData.Keys[num + 1] - this.m_PositionData.Keys[num]);
result = this.LinearInterpolate(this.m_PositionData.Values[num], this.m_PositionData.Values[num + 1], percent);
return(true);
}
}
catch
{
}
result.X = 0.0;
result.Y = 0.0;
result.Z = 0.0;
return(false);
}
public XYZCoord GetRinexSVPos(string filename, int svPRN, double TOW, out double ionoError, out double svClkBias, out XYZCoord svVel)
{
XYZCoord coord = new XYZCoord();
if (this.m_RinexFile.GetNumEphemerides() == 0)
{
this.m_RinexFile.Read(@"..\scripts\TunnelsMeasAndNav\brdc2140.07n");
}
RinexEph eph = this.m_RinexFile.SearchRinexArrayList((byte) svPRN, (int) TOW);
if (eph == null)
{
ionoError = 0.0;
svClkBias = 0.0;
svVel.X = svVel.Y = svVel.Z = 0.0;
return coord;
}
tSVD_SVState st = new tSVD_SVState();
tGPSTime gTime = new tGPSTime();
gTime.isTOWValid = true;
gTime.isWeekValid = true;
gTime.time = TOW;
gTime.week = (int) eph.weekNo;
new ComputeSVPos().computeSVState(st, (short) svPRN, eph, gTime);
coord.X = st.pos[0];
coord.Y = st.pos[1];
coord.Z = st.pos[2];
svVel.X = st.vel[0];
svVel.Y = st.vel[1];
svVel.Z = st.vel[2];
ionoError = st.ionoDelayStd;
svClkBias = st.clockBias;
return coord;
}
public XYZCoord GetIMUTruePos(string filename, double TOW)
{
XYZCoord result = new XYZCoord();
if (this.m_ImuFile.GetNumSamples() == 0)
{
this.m_ImuFile.LoadFromFile(filename);
}
this.m_ImuFile.GetPositionAtTime(TOW, out result);
return result;
}
public double ComputePRerrorOneSV(int svPRN, double svTOW, double svPseudoRange, double ionoErr, double clkBias, string RinexFileName, string IMUFileName, out string strPRtrue_ClkBiasRange_txTOW)
{
double num6;
double num7;
double num = 0.0;
strPRtrue_ClkBiasRange_txTOW = "-1.0,-1.0,-1.0";
this.m_SV_PRN = svPRN;
this.m_SV_TOW = svTOW;
double num2 = svPseudoRange / 299792458.0;
this.m_SV_TxTOW = (this.m_SV_TOW - num2) - clkBias;
double ionoError = 0.0;
double svClkBias = 0.0;
XYZCoord svVel = new XYZCoord();
XYZCoord a = this.GetRinexSVPos(RinexFileName, this.m_SV_PRN, this.m_SV_TxTOW, out ionoError, out svClkBias, out svVel);
XYZCoord iMUTruePos = this.GetIMUTruePos(IMUFileName, svTOW);
XYZCoord coord4 = new XYZCoord();
if ((a == coord4) || (iMUTruePos == coord4))
{
return -1.0;
}
double num5 = XYZCoord.getDistance(a, iMUTruePos);
double[] svPos = new double[] { a.X, a.Y, a.Z };
double[] navPos = new double[] { iMUTruePos.X, iMUTruePos.Y, iMUTruePos.Z };
double[] numArray3 = new double[] { svVel.X, svVel.Y, svVel.Z };
computeTropo tropo = new computeTropo();
GPSUtilsClass.ComputeAzEl(navPos, svPos, out num6, out num7);
double alt = GPSUtilsClass.ComputeApproxAltitude(navPos);
double num9 = tropo.NL_ComputeTropo(alt, num7);
double num10 = computeEarthRotation.earthRotationCorrection(svPos, navPos, numArray3);
double num11 = num5 - num10;
double num12 = (svPseudoRange - (clkBias * 299792458.0)) + (svClkBias * 299792458.0);
strPRtrue_ClkBiasRange_txTOW = string.Format("{0}, {1}, {2}", num5, num12, this.m_SV_TxTOW);
num = num5 - num12;
return (((num - ionoErr) - num9) - num11);
}
public static double getDistance(XYZCoord a, XYZCoord b)
{
double d = Math.Pow(a.X - b.X, 2.0) + Math.Pow(a.Y - b.Y, 2.0);
d += Math.Pow(a.Z - b.Z, 2.0);
return Math.Sqrt(d);
}
private XYZCoord LinearInterpolate(XYZCoord in1, XYZCoord in2, double percent)
{
XYZCoord coord;
GPSUtilsClass class2 = new GPSUtilsClass();
double[] numArray = new double[] { in1.X, in1.Y, in1.Z };
double[] numArray2 = new double[] { in2.X, in2.Y, in2.Z };
double[] numArray3 = new double[3];
numArray3 = class2.LinearInterpolateXYZ(numArray, numArray2, percent);
coord.X = numArray3[0];
coord.Y = numArray3[1];
coord.Z = numArray3[2];
return coord;
}
private bool GetPositionAtTimeBetweenIndexes(double inTime, int low_idx, int high_idx, out XYZCoord result)
{
try
{
if ((inTime >= this.m_PositionData.Keys[low_idx]) && (inTime <= this.m_PositionData.Keys[high_idx]))
{
int num = low_idx;
while (num < high_idx)
{
if (inTime <= this.m_PositionData.Keys[num])
{
break;
}
num++;
}
num--;
this.m_current_idx = num;
double percent = (100.0 * (inTime - this.m_PositionData.Keys[num])) / (this.m_PositionData.Keys[num + 1] - this.m_PositionData.Keys[num]);
result = this.LinearInterpolate(this.m_PositionData.Values[num], this.m_PositionData.Values[num + 1], percent);
return true;
}
}
catch
{
}
result.X = 0.0;
result.Y = 0.0;
result.Z = 0.0;
return false;
}
