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 ) );
}
double Track::GetXTE( double fm1Lat, double fm1Lon, double fm2Lat, double fm2Lon, double toLat, double toLon )
{
vector2D v, w, p;
// First we get the cartesian coordinates to the line endpoints, using
// the current position as origo.
double brg1, dist1, brg2, dist2;
DistanceBearingMercator( toLat, toLon, fm1Lat, fm1Lon, &brg1, &dist1 );
w.x = dist1 * sin( brg1 * PI / 180. );
w.y = dist1 * cos( brg1 * PI / 180. );
DistanceBearingMercator( toLat, toLon, fm2Lat, fm2Lon, &brg2, &dist2 );
v.x = dist2 * sin( brg2 * PI / 180. );
v.y = dist2 * cos( brg2 * PI / 180. );
p.x = 0.0; p.y = 0.0;
const double lengthSquared = _distance2( v, w );
if ( lengthSquared == 0.0 ) {
// v == w case
return _distance( p, v );
}
// Consider the line extending the segment, parameterized as v + t (w - v).
// We find projection of origo onto the line.
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
vector2D a = p - v;
vector2D b = w - v;
double t = vDotProduct( &a, &b ) / lengthSquared;
if (t < 0.0) return _distance(p, v); // Beyond the 'v' end of the segment
else if (t > 1.0) return _distance(p, w); // Beyond the 'w' end of the segment
vector2D projection = v + t * (w - v); // Projection falls on the segment
return _distance(p, projection);
}
