public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp)
{
double lat_deg, lon_deg, rErr;
int it_Cnt;
if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(),
targetLocation.Latitude, targetLocation.Longitude, depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(),
targetLocation.Latitude, targetLocation.Longitude, depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid, soundSpeed,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
if (ExternalSolver != null)
{
ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
throw new NullReferenceException("ExternalSolver not defined");
}
}
if (rErr < radialErrorThreshold)
{
crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg));
if (crsEstimator.IsCourse)
{
TargetCourseUpdatedHandler.Rise(this,
new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp));
}
targetLocation.Latitude = lat_deg;
targetLocation.Longitude = lon_deg;
targetLocationTS = timeStamp;
TargetLocationUpdatedHandler.Rise(this,
new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp));
TargetLocationUpdatedExHandler.Rise(this,
new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp));
}
else
{
RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs());
}
}
public void ProcessBasePoints(IEnumerable <T> basePoints, DateTime timeStamp)
{
double lat_deg, lon_deg, rErr;
int it_Cnt;
if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(),
targetLocation.Latitude, targetLocation.Longitude, targetLocation.Depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(),
targetLocation.Latitude, targetLocation.Longitude, targetLocation.Depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid, soundSpeed,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
if (ExternalSolver != null)
{
ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
throw new NullReferenceException("ExternalSolver not defined");
}
}
if (rErr < radialErrorThreshold)
{
if (IsTargetLocation)
{
var duration = timeStamp.Subtract(targetLocationTS);
if (duration.TotalSeconds <= maxIntervalToSpeedEstimationSec)
{
var prevLocRad = GeoPoint3D.ToRad(targetLocation);
double currLocLatRad = Algorithms.Deg2Rad(lat_deg);
double currLocLonRad = Algorithms.Deg2Rad(lon_deg);
double d_passed_m = 0, course_fwd_rad = 0, course_rev_rad = 0;
int its = 0;
Algorithms.VincentyInverse(prevLocRad.Latitude, prevLocRad.Longitude,
currLocLatRad, currLocLonRad, referenceEllipsoid,
Algorithms.VNC_DEF_EPSILON, Algorithms.VNC_DEF_IT_LIMIT,
out d_passed_m, out course_fwd_rad, out course_rev_rad, out its);
double estTargetCourse = Algorithms.Rad2Deg(course_fwd_rad);
double estTargetSpeed = d_passed_m / duration.TotalSeconds;
TargetCourseSpeedAndCourseUpdatedHandler.Rise(this,
new TargetCourseAndSpeedUpdatedEventArgs(estTargetCourse, estTargetSpeed, timeStamp));
}
}
targetLocation.Latitude = lat_deg;
targetLocation.Longitude = lon_deg;
targetLocationTS = timeStamp;
TargetLocationUpdatedHandler.Rise(this,
new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp));
}
else
{
RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs());
}
}
public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp)
{
double lat_deg, lon_deg, rErr;
int it_Cnt;
if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(),
targetLocation.Latitude, targetLocation.Longitude, depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T)))
{
UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(),
targetLocation.Latitude, targetLocation.Longitude, depth,
Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize,
referenceEllipsoid, soundSpeed,
out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
if (ExternalSolver != null)
{
ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt);
}
else
{
throw new NullReferenceException("ExternalSolver not defined");
}
}
if (rErr < radialErrorThreshold)
{
crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg));
if (crsEstimator.IsCourse)
{
TargetCourseUpdatedHandler.Rise(this,
new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp));
}
targetLocation.Latitude = lat_deg;
targetLocation.Longitude = lon_deg;
targetLocationTS = timeStamp;
TargetLocationUpdatedHandler.Rise(this,
new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp));
TargetLocationUpdatedExHandler.Rise(this,
new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp));
TBAQuality tbaState = Navigation.GetTBAState(Navigation.GetBasesMaxAngularGapDeg(basePoints, lat_deg, lon_deg));
DOPState dopState = DOPState.Invalid;
double gdop = double.NaN, pdop = double.NaN, hdop = double.NaN, vdop = double.NaN, tdop = double.NaN;
GeoPoint3D tL = new GeoPoint3D(targetLocation.Latitude, targetLocation.Longitude, depth);
if (Navigation.GetDOPs(basePoints, tL, Algorithms.WGS84Ellipsoid, out gdop, out pdop, out hdop, out vdop, out tdop))
{
dopState = Navigation.GetDOPState(hdop);
}
BaseQualityUpdatedHandler.Rise(this,
new BaseQualityUpdatedEventArgs(tbaState, gdop, pdop, hdop, vdop, tdop, dopState));
}
else
{
RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs());
}
}
