Added a distance off route routine (works best with 2d cartesian coordinates.)

diff --git a/simgear/math/vector.cxx b/simgear/math/vector.cxx
index 637ea4bc0803e8507e631828a6a69811bb34e66a..c67ff8ff13ab4d454d89abb4f9a80ff283629401 100644 (file)
--- a/simgear/math/vector.cxx
+++ b/simgear/math/vector.cxx
@@ -30,10 +30,54 @@
 #include "vector.hxx"
 
 
+// Given a point p, and a line through p0 with direction vector d,
+// find the closest point (p1) on the line
+void sgClosestPointToLine( sgVec3 p1, const sgVec3 p, const sgVec3 p0,
+                          const sgVec3 d ) {
+
+    sgVec3 u, u1;
+    
+    // u = p - p0
+    sgSubVec3(u, p, p0);
+
+    // calculate the projection, u1, of u along d.
+    // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
+    sgScaleVec3( u1, d, sgScalarProductVec3(u,d) / sgScalarProductVec3(d,d) );
+
+    // calculate the point p1 along the line that is closest to p
+    // p0 = p1 + u1
+    sgAddVec3(p1, p0, u1);
+}
+
+
+// Given a point p, and a line through p0 with direction vector d,
+// find the closest point (p1) on the line
+void sgdClosestPointToLine( sgdVec3 p1, const sgdVec3 p, const sgdVec3 p0,
+                           const sgdVec3 d ) {
+
+    sgdVec3 u, u1;
+    
+    // u = p - p0
+    sgdSubVec3(u, p, p0);
+
+    // calculate the projection, u1, of u along d.
+    // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
+    double ud = sgdScalarProductVec3(u, d);
+    double dd = sgdScalarProductVec3(d, d);
+    double tmp = ud / dd;
+
+    sgdScaleVec3(u1, d, tmp);;
+
+    // calculate the point p1 along the line that is closest to p
+    // p0 = p1 + u1
+    sgdAddVec3(p1, p0, u1);
+}
+
+
 // Given a point p, and a line through p0 with direction vector d,
 // find the shortest distance (squared) from the point to the line
-double sgPointLineDistSquared( const sgVec3 p, const sgVec3 p0,
-                              const sgVec3 d ) {
+double sgClosestPointToLineDistSquared( const sgVec3 p, const sgVec3 p0,
+                                       const sgVec3 d ) {
 
     sgVec3 u, u1, v;
     
@@ -54,20 +98,19 @@ double sgPointLineDistSquared( const sgVec3 p, const sgVec3 p0,
 
 // Given a point p, and a line through p0 with direction vector d,
 // find the shortest distance (squared) from the point to the line
-double sgdPointLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
-                               const sgdVec3 d ) {
+double sgdClosestPointToLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
+                                        const sgdVec3 d ) {
 
     sgdVec3 u, u1, v;
-    double ud, dd, tmp;
     
     // u = p - p0
     sgdSubVec3(u, p, p0);
 
     // calculate the projection, u1, of u along d.
     // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
-    ud = sgdScalarProductVec3(u, d);
-    dd = sgdScalarProductVec3(d, d);
-    tmp = ud / dd;
+    double ud = sgdScalarProductVec3(u, d);
+    double dd = sgdScalarProductVec3(d, d);
+    double tmp = ud / dd;
 
     sgdScaleVec3(u1, d, tmp);;
 

diff --git a/simgear/math/vector.hxx b/simgear/math/vector.hxx
index ef97e7de35af8134b0226c68bb00e8f11fb3e20d..84029aa35a97e38c111adf6159a43d3b239add4c 100644 (file)
--- a/simgear/math/vector.hxx
+++ b/simgear/math/vector.hxx
@@ -92,15 +92,25 @@ inline void sgCopyNegateVec4( sgVec4 dst, sgVec4 src )
        dst [ 3 ] = -src [ 3 ] ;
 }
 
+// Given a point p, and a line through p0 with direction vector d,
+// find the closest point (p1) on the line
+void sgClosestPointToLine( sgVec3 p1, const sgVec3 p, const sgVec3 p0,
+                          const sgVec3 d );
+
+// Given a point p, and a line through p0 with direction vector d,
+// find the closest point (p1) on the line
+void sgdClosestPointToLine( sgdVec3 p1, const sgdVec3 p, const sgdVec3 p0,
+                           const sgdVec3 d );
+
 // Given a point p, and a line through p0 with direction vector d,
 // find the shortest distance (squared) from the point to the line
-double sgPointLineDistSquared( const sgVec3 p, const sgVec3 p0,
-                              const sgVec3 d );
+double sgClosestPointToLineDistSquared( const sgVec3 p, const sgVec3 p0,
+                                       const sgVec3 d );
 
 // Given a point p, and a line through p0 with direction vector d,
 // find the shortest distance (squared) from the point to the line
-double sgdPointLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
-                               const sgdVec3 d );
+double sgdClosestPointToLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
+                                        const sgdVec3 d );
 
 // This is same as
 // sgMakeMatTrans4( sgMat4 sgTrans, sgVec3 trans )

diff --git a/simgear/route/Makefile.am b/simgear/route/Makefile.am
index 540a31dfbc37d181e58c0d626754f7e05864bcdf..b35100708a2300487667ead1faac162d43581bf5 100644 (file)
--- a/simgear/route/Makefile.am
+++ b/simgear/route/Makefile.am
@@ -10,11 +10,18 @@ libsgroute_a_SOURCES = \
 
 INCLUDES += -I$(top_srcdir)
 
-noinst_PROGRAMS = waytest
+noinst_PROGRAMS = waytest routetest
 
 waytest_SOURCES = waytest.cxx
 
 waytest_LDADD = \
+       $(top_builddir)/simgear/route/libsgroute.a \
+       $(top_builddir)/simgear/math/libsgmath.a \
+       $(top_builddir)/simgear/debug/libsgdebug.a
+
+routetest_SOURCES = routetest.cxx
+
+routetest_LDADD = \
        $(top_builddir)/simgear/route/libsgroute.a \
        $(top_builddir)/simgear/math/libsgmath.a \
        $(top_builddir)/simgear/debug/libsgdebug.a
\ No newline at end of file

diff --git a/simgear/route/route.cxx b/simgear/route/route.cxx
index 6052e0d7612d5e6b692b4522971a37d0dd48170f..24264725158994ec6ac238c6df3c5832982b753e 100644 (file)
--- a/simgear/route/route.cxx
+++ b/simgear/route/route.cxx
@@ -21,6 +21,10 @@
 // $Id$
 
 
+#include <plib/sg.h>
+
+#include <simgear/math/vector.hxx>
+
 #include "route.hxx"
 
 
@@ -33,3 +37,34 @@ SGRoute::SGRoute() {
 // destructor
 SGRoute::~SGRoute() {
 }
+
+
+// Calculate perpendicular distance from the current route segment
+// This routine assumes all points are laying on a flat plane and
+// ignores the altitude (or Z) dimension.  For best results, use with
+// CARTESIAN way points.
+double SGRoute::distance_off_route( double x, double y ) const {
+    if ( current_wp > 0 ) {
+       int n0 = current_wp - 1;
+       int n1 = current_wp;
+       sgdVec3 p, p0, p1, d;
+       sgdSetVec3( p, x, y, 0.0 );
+       sgdSetVec3( p0, 
+                   route[n0].get_target_lon(), route[n0].get_target_lat(),
+                   0.0 );
+       sgdSetVec3( p1,
+                   route[n1].get_target_lon(), route[n1].get_target_lat(),
+                   0.0 );
+       sgdSubVec3( d, p0, p1 );
+
+       return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
+
+    } else {
+       // We are tracking the first waypoint so there is no route
+       // segment.  If you add the current location as the first
+       // waypoint and the actual waypoint as the second, then we
+       // will have a route segment and calculate distance from it.
+
+       return 0;
+    }
+}

diff --git a/simgear/route/route.hxx b/simgear/route/route.hxx
index ead338d42de9cc4bb3b14c188772cddae0907de2..2fe2edc142fbfc61743ac1a177c2aac22472f334 100644 (file)
--- a/simgear/route/route.hxx
+++ b/simgear/route/route.hxx
@@ -127,6 +127,12 @@ public:
            route.erase( route.begin() );
        }
     }
+
+    // Calculate perpendicular distance from the current route segment
+    // This routine assumes all points are laying on a flat plane and
+    // ignores the altitude (or Z) dimension.  For best results, use
+    // with CARTESIAN way points.
+    double distance_off_route( double x, double y ) const;
 };
 
 

diff --git a/simgear/route/routetest.cxx b/simgear/route/routetest.cxx
new file mode 100644 (file)
index 0000000..2de066e
--- /dev/null
+++ b/simgear/route/routetest.cxx
@@ -0,0 +1,27 @@
+#include <simgear/constants.h>
+
+#include "route.hxx"
+#include "waypoint.hxx"
+
+int main() {
+    SGRoute route;
+
+    route.add_waypoint( SGWayPoint(0, 0, 0, SGWayPoint::CARTESIAN, "Start") );
+    route.add_waypoint( SGWayPoint(1, 0, 0, SGWayPoint::CARTESIAN, "1") );
+    route.add_waypoint( SGWayPoint(2, 0, 0, SGWayPoint::CARTESIAN, "2") );
+    route.add_waypoint( SGWayPoint(2, 2, 0, SGWayPoint::CARTESIAN, "3") );
+    route.add_waypoint( SGWayPoint(4, 2, 0, SGWayPoint::CARTESIAN, "4") );
+   
+    route.set_current( 1 );
+
+    cout << "( 0.5, 0 ) = " << route.distance_off_route( 0.5, 0 ) << endl;
+    cout << "( 0.5, 1 ) = " << route.distance_off_route( 0.5, 1 ) << endl;
+    cout << "( 0.5, -1 ) = " << route.distance_off_route( 0.5, 1 ) << endl;
+
+    route.set_current( 3 );
+
+    cout << "( 2, 4 ) = " << route.distance_off_route( 2, 4 ) << endl;
+    cout << "( 2.5, 4 ) = " << route.distance_off_route( 2.5, 4 ) << endl;
+
+    return 0;
+}