double ODSelect::vGetLengthOfNormal( pODVector2D a, pODVector2D b, pODVector2D n )
{
ODvector2D c, vNormal;
vNormal.x = 0;
vNormal.y = 0;
//
//Obtain projection vector.
//
//c = ((a * b)/(|b|^2))*b
//
c.x = b->x * ( vDotProduct( a, b ) / vDotProduct( b, b ) );
c.y = b->y * ( vDotProduct( a, b ) / vDotProduct( b, b ) );
//
//Obtain perpendicular projection : e = a - c
//
vSubtractVectors( a, &c, &vNormal );
//
//Fill PROJECTION structure with appropriate values.
//
*n = vNormal;
return ( vVectorMagnitude( &vNormal ) );
}
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;
}