bool ODSelect::IsSegmentSelected( float a, float b, float c, float d, float slat, float slon )
{
double adder = 0.;
if( ( c * d ) < 0. ) {
// Arrange for points to be increasing longitude, c to d
double dist, brg;
DistanceBearingMercator_Plugin( a, c, b, d, &brg, &dist );
if( brg < 180. ) // swap points?
{
double tmp;
tmp = c;
c = d;
d = tmp;
tmp = a;
a = b;
b = tmp;
}
if( d < 0. ) // idl?
{
d += 360.;
if( slon < 0. ) adder = 360.;
}
}
// As a course test, use segment bounding box test
if( ( slat >= ( fmin ( a,b ) - selectRadius ) ) && ( slat <= ( fmax ( a,b ) + selectRadius ) )
&& ( ( slon + adder ) >= ( fmin ( c,d ) - selectRadius ) )
&& ( ( slon + adder ) <= ( fmax ( c,d ) + selectRadius ) ) ) {
// Use vectors to do hit test....
ODvector2D va, vb, vn;
// Assuming a Mercator projection
double ap, cp;
toSM( a, c, 0., 0., &cp, &ap );
double bp, dp;
toSM( b, d, 0., 0., &dp, &bp );
double slatp, slonp;
toSM( slat, slon + adder, 0., 0., &slonp, &slatp );
va.x = slonp - cp;
va.y = slatp - ap;
vb.x = dp - cp;
vb.y = bp - ap;
double delta = vGetLengthOfNormal( &va, &vb, &vn );
if( fabs( delta ) < ( selectRadius * 1852 * 60 ) ) return true;
}
return false;
}
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;
}
