From c39e08cb2a8d5666f9d54af7d243fa525214d24e Mon Sep 17 00:00:00 2001 From: curt Date: Mon, 23 Oct 2000 14:57:29 +0000 Subject: [PATCH] Added a distance off route routine (works best with 2d cartesian coordinates.) --- simgear/math/vector.cxx | 59 ++++++++++++++++++++++++++++++++----- simgear/math/vector.hxx | 20 +++++++++---- simgear/route/Makefile.am | 9 +++++- simgear/route/route.cxx | 35 ++++++++++++++++++++++ simgear/route/route.hxx | 6 ++++ simgear/route/routetest.cxx | 27 +++++++++++++++++ 6 files changed, 142 insertions(+), 14 deletions(-) create mode 100644 simgear/route/routetest.cxx diff --git a/simgear/math/vector.cxx b/simgear/math/vector.cxx index 637ea4bc..c67ff8ff 100644 --- 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 ef97e7de..84029aa3 100644 --- a/simgear/math/vector.hxx +++ b/simgear/math/vector.hxx @@ -93,14 +93,24 @@ inline void sgCopyNegateVec4( sgVec4 dst, sgVec4 src ) } // 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 ); +// 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 sgdPointLineDistSquared( const sgdVec3 p, const sgdVec3 p0, - const sgdVec3 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 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 540a31df..b3510070 100644 --- 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 6052e0d7..24264725 100644 --- a/simgear/route/route.cxx +++ b/simgear/route/route.cxx @@ -21,6 +21,10 @@ // $Id$ +#include + +#include + #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 ead338d4..2fe2edc1 100644 --- 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 index 00000000..2de066e7 --- /dev/null +++ b/simgear/route/routetest.cxx @@ -0,0 +1,27 @@ +#include + +#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; +}