static double chart_dist(int index)
{
double d;
float clon;
float clat;
const ChartTableEntry &cte = ChartData->GetChartTableEntry(index);
// if the chart contains ownship position set the distance to 0
if (cte.GetBBox().Contains(gLon, gLat))
d = 0.;
else {
// find the nearest edge
double t;
clon = (cte.GetLonMax() + cte.GetLonMin())/2;
d = DistGreatCircle(cte.GetLatMax(), clon, gLat, gLon);
t = DistGreatCircle(cte.GetLatMin(), clon, gLat, gLon);
if (t < d)
d = t;
clat = (cte.GetLatMax() + cte.GetLatMin())/2;
t = DistGreatCircle(clat, cte.GetLonMin(), gLat, gLon);
if (t < d)
d = t;
t = DistGreatCircle(clat, cte.GetLonMax(), gLat, gLon);
if (t < d)
d = t;
}
return d;
}
void Route::AddPoint( RoutePoint *pNewPoint, bool b_rename_in_sequence, bool b_deferBoxCalc )
{
if( pNewPoint->m_bIsolatedMark ) {
pNewPoint->m_bKeepXRoute = true;
}
pNewPoint->m_bIsolatedMark = false; // definitely no longer isolated
pNewPoint->m_bIsInRoute = true;
pRoutePointList->Append( pNewPoint );
m_nPoints++;
if( !b_deferBoxCalc ) CalculateBBox();
if( m_pLastAddedPoint ) pNewPoint->m_seg_len = DistGreatCircle( m_pLastAddedPoint->m_lat,
m_pLastAddedPoint->m_lon, pNewPoint->m_lat, pNewPoint->m_lon );
m_route_length += pNewPoint->m_seg_len;
m_pLastAddedPoint = pNewPoint;
if( b_rename_in_sequence && pNewPoint->GetName().IsEmpty() && !pNewPoint->m_bKeepXRoute ) {
wxString name;
name.Printf( _T ( "%03d" ), m_nPoints );
pNewPoint->SetName( name );
pNewPoint->m_bDynamicName = true;
}
return;
}
void ActiveTrack::OnTimerTrack( wxTimerEvent& event )
{
m_TimerTrack.Stop();
m_track_run++;
if( m_lastStoredTP )
m_prev_dist = DistGreatCircle( gLat, gLon, m_lastStoredTP->m_lat, m_lastStoredTP->m_lon );
else
m_prev_dist = 999.0;
bool b_addpoint = false;
if( ( m_TrackTimerSec > 0. ) && ( (double) m_track_run >= m_TrackTimerSec )
&& ( m_prev_dist > m_minTrackpoint_delta ) ) {
b_addpoint = true;
m_track_run = 0;
}
if( b_addpoint )
AddPointNow();
else //continuously update track beginning point timestamp if no movement.
if( ( trackPointState == firstPoint ) && !g_bTrackDaily )
{
wxDateTime now = wxDateTime::Now();
if(TrackPoints.empty())
TrackPoints.front()->SetCreateTime(now.ToUTC());
}
m_TimerTrack.Start( 1000, wxTIMER_CONTINUOUS );
}
double Track::Length()
{
TrackPoint *l = NULL;
double total = 0.0;
for(size_t i = 0; i < TrackPoints.size(); i++) {
TrackPoint *t = TrackPoints[i];
if(l) {
const double offsetLat = 1e-6;
const double deltaLat = l->m_lat - t->m_lat;
if ( fabs( deltaLat ) > offsetLat )
total += DistGreatCircle( l->m_lat, l->m_lon, t->m_lat, t->m_lon );
else
total += DistGreatCircle( l->m_lat + copysign( offsetLat, deltaLat ), l->m_lon, t->m_lat, t->m_lon );
}
l = t;
}
return total;
}
void ActiveTrack::Stop( bool do_add_point )
{
if(m_bRunning){
if(do_add_point)
AddPointNow( true ); // Force add last point
else{
double delta = 0.0;
if( m_lastStoredTP )
delta = DistGreatCircle( gLat, gLon, m_lastStoredTP->m_lat, m_lastStoredTP->m_lon );
if( delta > m_minTrackpoint_delta )
AddPointNow( true ); // Add last point
}
}
m_TimerTrack.Stop();
m_bRunning = false;
m_track_run = 0;
}
/// New functions
void DistanceBearingMercator(double lat0, double lon0, double lat1, double lon1, double *dist, double *brg)
{
// Calculate bearing by conversion to SM (Mercator) coordinates, then simple trigonometry
double lon0x = lon0;
double lon1x = lon1;
// Make lon points the same phase
if ((lon0x * lon1x) < 0.)
{
lon0x < 0.0 ? lon0x += 360.0 : lon1x += 360.0;
// Choose the shortest distance
if (fabs(lon0x - lon1x) > 180.)
{
lon0x > lon1x ? lon0x -= 360.0 : lon1x -= 360.0;
}
// Make always positive
lon1x += 360.;
lon0x += 360.;
}
// Classic formula, which fails for due east/west courses....
if (dist)
{
// In the case of exactly east or west courses
// we must make an adjustment if we want true Mercator distances
// This idea comes from Thomas(Cagney)
// We simply require the dlat to be (slightly) non-zero, and carry on.
// MAS022210 for HamishB from 1e-4 && .001 to 1e-9 for better precision
// on small latitude diffs
const double mlat0 = fabs(lat1 - lat0) < 1e-9 ? lat0 + 1e-9 : lat0;
double east, north;
toSM_ECC(lat1, lon1x, mlat0, lon0x, &east, &north);
const double C = atan2(east, north);
if (cos(C))
{
const double dlat = (lat1 - mlat0) * 60.; // in minutes
*dist = (dlat / cos(C));
}
else
{
*dist = DistGreatCircle(lat0, lon0, lat1, lon1);
}
}
// Calculate the bearing using the un-adjusted original latitudes and Mercator Sailing
if (brg)
{
double east, north;
toSM_ECC(lat1, lon1x, lat0, lon0x, &east, &north);
const double C = atan2(east, north);
const double brgt = 180. + (C * 180. / PI);
if (brgt < 0)
*brg = brgt + 360.;
else if (brgt >= 360.)
*brg = brgt - 360.;
else
*brg = brgt;
}
}
void DistanceBearingMercator(double lat0, double lon0, double lat1, double lon1, double *brg, double *dist)
{
double east, north, brgt, C;
double lon0x, lon1x, dlat;
double mlat0;
// Calculate bearing by conversion to SM (Mercator) coordinates, then simple trigonometry
lon0x = lon0;
lon1x = lon1;
// Make lon points the same phase
if((lon0x * lon1x) < 0.)
{
if(lon0x < 0.)
lon0x += 360.;
else
lon1x += 360.;
// Choose the shortest distance
if(fabs(lon0x - lon1x) > 180.)
{
if(lon0x > lon1x)
lon0x -= 360.;
else
lon1x -= 360.;
}
// Make always positive
lon1x += 360.;
lon0x += 360.;
}
// In the case of exactly east or west courses
// we must make an adjustment if we want true Mercator distances
// This idea comes from Thomas(Cagney)
// We simply require the dlat to be (slightly) non-zero, and carry on.
// MAS022210 for HamishB from 1e-4 && .001 to 1e-9 for better precision
// on small latitude diffs
mlat0 = lat0;
if(fabs(lat1 - lat0) < 1e-9)
mlat0 += 1e-9;
toSM_ECC(lat1, lon1x, mlat0, lon0x, &east, &north);
C = atan2(east, north);
dlat = (lat1 - mlat0) * 60.; // in minutes
// Classic formula, which fails for due east/west courses....
if(dist)
{
if(cos(C))
*dist = (dlat /cos(C));
else
*dist = DistGreatCircle(lat0, lon0, lat1, lon1);
}
// Calculate the bearing using the un-adjusted original latitudes and Mercator Sailing
if(brg)
{
toSM_ECC(lat1, lon1x, lat0, lon0x, &east, &north);
C = atan2(east, north);
brgt = 180. + (C * 180. / PI);
if (brgt < 0)
brgt += 360.;
if (brgt > 360.)
brgt -= 360;
*brg = brgt;
}
// Alternative formulary
// From Roy Williams, "Geometry of Navigation", we have p = Dlo (Dlat/DMP) where p is the departure.
// Then distance is then:D = sqrt(Dlat^2 + p^2)
/*
double dlo = (lon1x - lon0x) * 60.;
double departure = dlo * dlat / ((north/1852.));
if(dist)
*dist = sqrt((dlat*dlat) + (departure * departure));
*/
}
bool Routeman::UpdateProgress()
{
bool bret_val = false;
if( pActiveRoute ) {
// Update bearing, range, and crosstrack error
// Bearing is calculated as Mercator Sailing, i.e. a cartographic "bearing"
double north, east;
toSM( pActivePoint->m_lat, pActivePoint->m_lon, gLat, gLon, &east, &north );
double a = atan( north / east );
if( fabs( pActivePoint->m_lon - gLon ) < 180. ) {
if( pActivePoint->m_lon > gLon ) CurrentBrgToActivePoint = 90. - ( a * 180 / PI );
else
CurrentBrgToActivePoint = 270. - ( a * 180 / PI );
} else {
if( pActivePoint->m_lon > gLon ) CurrentBrgToActivePoint = 270. - ( a * 180 / PI );
else
CurrentBrgToActivePoint = 90. - ( a * 180 / PI );
}
// Calculate range using Great Circle Formula
double d5 = DistGreatCircle( gLat, gLon, pActivePoint->m_lat, pActivePoint->m_lon );
CurrentRngToActivePoint = d5;
// Get the XTE vector, normal to current segment
vector2D va, vb, vn;
double brg1, dist1, brg2, dist2;
DistanceBearingMercator( pActivePoint->m_lat, pActivePoint->m_lon,
pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon, &brg1,
&dist1 );
vb.x = dist1 * sin( brg1 * PI / 180. );
vb.y = dist1 * cos( brg1 * PI / 180. );
DistanceBearingMercator( pActivePoint->m_lat, pActivePoint->m_lon, gLat, gLon, &brg2,
&dist2 );
va.x = dist2 * sin( brg2 * PI / 180. );
va.y = dist2 * cos( brg2 * PI / 180. );
double sdelta = vGetLengthOfNormal( &va, &vb, &vn ); // NM
CurrentXTEToActivePoint = sdelta;
// Calculate the distance to the arrival line, which is perpendicular to the current route segment
// Taking advantage of the calculated normal from current position to route segment vn
vector2D vToArriveNormal;
vSubtractVectors( &va, &vn, &vToArriveNormal );
CurrentRangeToActiveNormalCrossing = vVectorMagnitude( &vToArriveNormal );
// Compute current segment course
// Using simple Mercater projection
double x1, y1, x2, y2;
toSM( pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon,
pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon, &x1,
&y1 );
toSM( pActivePoint->m_lat, pActivePoint->m_lon, pActiveRouteSegmentBeginPoint->m_lat,
pActiveRouteSegmentBeginPoint->m_lon, &x2, &y2 );
double e1 = atan2( ( x2 - x1 ), ( y2 - y1 ) );
CurrentSegmentCourse = e1 * 180 / PI;
if( CurrentSegmentCourse < 0 ) CurrentSegmentCourse += 360;
// Compute XTE direction
double h = atan( vn.y / vn.x );
if( vn.x > 0 ) CourseToRouteSegment = 90. - ( h * 180 / PI );
else
CourseToRouteSegment = 270. - ( h * 180 / PI );
h = CurrentBrgToActivePoint - CourseToRouteSegment;
if( h < 0 ) h = h + 360;
if( h > 180 ) XTEDir = 1;
else
XTEDir = -1;
// Determine Arrival
bool bDidArrival = false;
if( CurrentRangeToActiveNormalCrossing <= pActiveRoute->GetRouteArrivalRadius() ) {
m_bArrival = true;
UpdateAutopilot();
bDidArrival = true;
if( !ActivateNextPoint( pActiveRoute, false ) ) // at the end?
{
Route *pthis_route = pActiveRoute;
DeactivateRoute( true ); // this is an arrival
if( pthis_route->m_bDeleteOnArrival ) {
pConfig->DeleteConfigRoute( pthis_route );
DeleteRoute( pthis_route );
if( pRoutePropDialog ) {
pRoutePropDialog->SetRouteAndUpdate( NULL );
pRoutePropDialog->UpdateProperties();
}
if( pRouteManagerDialog ) pRouteManagerDialog->UpdateRouteListCtrl();
}
}
}
if( !bDidArrival ) // Only once on arrival
UpdateAutopilot();
bret_val = true; // a route is active
}
m_bDataValid = true;
return bret_val;
}
void ActiveTrack::AddPointNow( bool do_add_point )
{
wxDateTime now = wxDateTime::Now();
if( m_prev_dist < 0.0005 ) // avoid zero length segs
if( !do_add_point ) return;
if( m_prev_time.IsValid() ) if( m_prev_time == now ) // avoid zero time segs
if( !do_add_point ) return;
vector2D gpsPoint( gLon, gLat );
// The dynamic interval algorithm will gather all track points in a queue,
// and analyze the cross track errors for each point before actually adding
// a point to the track.
switch( trackPointState ) {
case firstPoint: {
TrackPoint *pTrackPoint = AddNewPoint( gpsPoint, now.ToUTC() );
m_lastStoredTP = pTrackPoint;
trackPointState = secondPoint;
do_add_point = false;
break;
}
case secondPoint: {
vector2D pPoint( gLon, gLat );
skipPoints.push_back( pPoint );
skipTimes.push_back( now.ToUTC() );
trackPointState = potentialPoint;
break;
}
case potentialPoint: {
if( gpsPoint == skipPoints[skipPoints.size()-1] ) break;
unsigned int xteMaxIndex = 0;
double xteMax = 0;
// Scan points skipped so far and see if anyone has XTE over the threshold.
for( unsigned int i=0; i<skipPoints.size(); i++ ) {
double xte = GetXTE( m_lastStoredTP->m_lat, m_lastStoredTP->m_lon, gLat, gLon, skipPoints[i].lat, skipPoints[i].lon );
if( xte > xteMax ) {
xteMax = xte;
xteMaxIndex = i;
}
}
if( xteMax > m_allowedMaxXTE ) {
TrackPoint *pTrackPoint = AddNewPoint( skipPoints[xteMaxIndex], skipTimes[xteMaxIndex] );
pSelect->AddSelectableTrackSegment( m_lastStoredTP->m_lat, m_lastStoredTP->m_lon,
pTrackPoint->m_lat, pTrackPoint->m_lon,
m_lastStoredTP, pTrackPoint, this );
m_prevFixedTP = m_fixedTP;
m_fixedTP = m_removeTP;
m_removeTP = m_lastStoredTP;
m_lastStoredTP = pTrackPoint;
for( unsigned int i=0; i<=xteMaxIndex; i++ ) {
skipPoints.pop_front();
skipTimes.pop_front();
}
// Now back up and see if we just made 3 points in a straight line and the middle one
// (the next to last) point can possibly be eliminated. Here we reduce the allowed
// XTE as a function of leg length. (Half the XTE for very short legs).
if( GetnPoints() > 2 ) {
double dist = DistGreatCircle( m_fixedTP->m_lat, m_fixedTP->m_lon, m_lastStoredTP->m_lat, m_lastStoredTP->m_lon );
double xte = GetXTE( m_fixedTP, m_lastStoredTP, m_removeTP );
if( xte < m_allowedMaxXTE / wxMax(1.0, 2.0 - dist*2.0) ) {
TrackPoints.pop_back();
TrackPoints.pop_back();
TrackPoints.push_back( m_lastStoredTP );
pSelect->DeletePointSelectableTrackSegments( m_removeTP );
pSelect->AddSelectableTrackSegment( m_fixedTP->m_lat, m_fixedTP->m_lon,
m_lastStoredTP->m_lat, m_lastStoredTP->m_lon,
m_fixedTP, m_lastStoredTP, this );
delete m_removeTP;
m_removeTP = m_fixedTP;
m_fixedTP = m_prevFixedTP;
}
}
}
skipPoints.push_back( gpsPoint );
skipTimes.push_back( now.ToUTC() );
break;
}
}
// Check if this is the last point of the track.
if( do_add_point ) {
TrackPoint *pTrackPoint = AddNewPoint( gpsPoint, now.ToUTC() );
pSelect->AddSelectableTrackSegment( m_lastStoredTP->m_lat, m_lastStoredTP->m_lon,
pTrackPoint->m_lat, pTrackPoint->m_lon,
m_lastStoredTP, pTrackPoint, this );
}
m_prev_time = now;
}
