orca-robotics INTRODUCTION Overview Download and Install Documentation REPOSITORY Interfaces Drivers Libraries Utilities Software Map DEVELOPER Dashboard PEOPLE Contributors Users Project Download Mailing lists

ograytracer.h /* * Orca-Robotics Project: Components for robotics * http://orca-robotics.sf.net/ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, George Mathews * * This copy of Orca is licensed to you under the terms described in * the LICENSE file included in this distribution. * */ #ifndef HYDROOGMAP_OGRAYTRACER_H #define HYDROOGMAP_OGRAYTRACER_H #include <hydroogmap/ogmap.h> #include <hydroportability/sharedlib.h> namespace hydroogmap { // // The OgRayTracer stuff's fast and general, but maybe a bit difficult to understand. // For a simple use case, see simpleograytrace.h // class SOEXPORT SafeLosCellTracer { public: SafeLosCellTracer( const hydroogmap::OgMap &theMap, unsigned char occupiedThreshold=hydroogmap::CELL_UNKNOWN ) : theMap_(theMap), occupiedThreshold_(occupiedThreshold) {} inline bool traceCell(int x, int y) { if ( !theMap_.cellWithinMap( x, y ) || theMap_.gridCell(x,y) >= occupiedThreshold_ ) { // Occupied or outside the map: stop tracing. lastEndX_ = x; lastEndY_ = y; return false; } // Keep tracing. return true; } inline void traceSucceeded( int x, int y ) { lastEndX_ = x; lastEndY_ = y; } int lastEndX() const { return lastEndX_; } int lastEndY() const { return lastEndY_; } private: const hydroogmap::OgMap &theMap_; const unsigned char occupiedThreshold_; // The coordinates of the end of the last ray-trace. int lastEndX_; int lastEndY_; }; class SOEXPORT UncheckedLosCellTracer { public: UncheckedLosCellTracer(const hydroogmap::OgMap &theMap, unsigned char occupiedThreshold=hydroogmap::CELL_UNKNOWN ) : theMap_(theMap), occupiedThreshold_(occupiedThreshold) {} inline bool traceCell(int x, int y) { if ( theMap_.gridCell(x,y) > occupiedThreshold_ ) { // Occupied: stop tracing. lastEndX_ = x; lastEndY_ = y; return false; } // Keep tracing. return true; } inline void traceSucceeded( int x, int y ) { lastEndX_ = x; lastEndY_ = y; } int lastEndX() const { return lastEndX_; } int lastEndY() const { return lastEndY_; } private: const hydroogmap::OgMap &theMap_; const unsigned char occupiedThreshold_; // The coordinates of the end of the last ray-trace. int lastEndX_; int lastEndY_; }; template<class CellTracer=SafeLosCellTracer> class SOEXPORT OgRayTracer { public: // non-const version OgRayTracer( CellTracer &cellTracer ) : cellTracer_(cellTracer) {} bool trace( int x0, int y0, int x1, int y1 ); private: // These guys return: // true: we traced out to (x1,y1) // false: the trace was short-circuited // bool rayTraceOct0( int x0, int y0, int x1, int y1 ); bool rayTraceOct1( int x0, int y0, int x1, int y1 ); bool rayTraceOct2( int x0, int y0, int x1, int y1 ); bool rayTraceOct3( int x0, int y0, int x1, int y1 ); bool rayTraceOct4( int x0, int y0, int x1, int y1 ); bool rayTraceOct5( int x0, int y0, int x1, int y1 ); bool rayTraceOct6( int x0, int y0, int x1, int y1 ); bool rayTraceOct7( int x0, int y0, int x1, int y1 ); bool rayTraceOct8( int x0, int y0, int x1, int y1 ); CellTracer &cellTracer_; }; template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::trace( int x0, int y0, int x1, int y1 ) { /* * Octants are defined as follows: * * \2|1/ * 3\|/0 * --x-- * 4/|\7 * /5|6\ * */ float slope; if ( x0 == x1 ) slope = 999999; else slope = (float)(y1-y0) / (float)(x1-x0); bool traceWentToEnd; if ( slope >= 0.0 && slope <= 1.0 ) { if ( x1 > x0 ) { // octant 0 traceWentToEnd = rayTraceOct0( x0, y0, x1, y1 ); } else { // octant 4 traceWentToEnd = rayTraceOct4( x1, y1, x0, y0 ); } } else if ( slope > 1.0 ) { if ( y1 > y0 ) { // octant 1 traceWentToEnd = rayTraceOct1( x0, y0, x1, y1 ); } else { // octant 5 traceWentToEnd = rayTraceOct5( x1, y1, x0, y0 ); } } else if ( slope <= -1.0 ) { if ( y1 > y0 ) { // octant 2 traceWentToEnd = rayTraceOct2( x0, y0, x1, y1 ); } else { // octant 6 traceWentToEnd = rayTraceOct6( x1, y1, x0, y0 ); } } else // ( slope < 0.0 && slope > -1.0 ) { if ( x1 > x0 ) { // octant 7 traceWentToEnd = rayTraceOct7( x0, y0, x1, y1 ); } else { // octant 3 traceWentToEnd = rayTraceOct3( x1, y1, x0, y0 ); } } if ( traceWentToEnd ) { cellTracer_.traceSucceeded( x1, y1 ); } return traceWentToEnd; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct0( int x0, int y0, int x1, int y1 ) { int y = y0; int dx = 2*(x1-x0); int dy = 2*(y1-y0); int dydx = dy-dx; int F = (dy-dx)/2; for (int x=x0; x<=x1; x++) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dy; } else { y++; F += dydx; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct4( int x0, int y0, int x1, int y1 ) { int y = y1; int dx = 2*(x1-x0); int dy = 2*(y1-y0); int dydx = dy-dx; int F = (dy-dx)/2; for (int x=x1; x>=x0; x--) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dy; } else { y--; F += dydx; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct1( int x0, int y0, int x1, int y1 ) { int x = x0; int dx = 2*(x1-x0); int dy = 2*(y1-y0); int dxdy = dx-dy; int F = (dx-dy)/2; for (int y=y0; y<=y1; y++) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dx; } else { x++; F += dxdy; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct5( int x0, int y0, int x1, int y1 ) { int x = x1; int dx = 2*(x1-x0); int dy = 2*(y1-y0); int dxdy = dx-dy; int F = (dx-dy)/2; for (int y=y1; y>=y0; y--) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dx; } else { x--; F += dxdy; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct2( int x0, int y0, int x1, int y1 ) { int x = x0; int dx = -2*(x1-x0); int dy = 2*(y1-y0); int dxdy = dx-dy; int F = (dx-dy)/2; for (int y=y0; y<=y1; y++) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dx; } else { x--; F += dxdy; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct6( int x0, int y0, int x1, int y1 ) { int x = x1; int dx = -2*(x1-x0); int dy = 2*(y1-y0); int dxdy = dx-dy; int F = (dx-dy)/2; for (int y=y1; y>=y0; y--) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dx; } else { x++; F += dxdy; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct7( int x0, int y0, int x1, int y1 ) { int y = y0; int dx = 2*(x1-x0); int dy = -2*(y1-y0); int dydx = dy-dx; int F = (dy-dx)/2; for (int x=x0; x<=x1; x++) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dy; } else { y--; F += dydx; } } return true; } template<class CellTracer> SOEXPORT bool OgRayTracer<CellTracer>::rayTraceOct3( int x0, int y0, int x1, int y1 ) { int y = y1; int dx = 2*(x1-x0); int dy = -2*(y1-y0); int dydx = dy-dx; int F = (dy-dx)/2; for (int x=x1; x>=x0; x--) { if ( !cellTracer_.traceCell( x, y ) ) { return false; } if ( F<0 ) { F += dy; } else { y++; F += dydx; } } return true; } } #endif

Webmaster: Tobias Kaupp (tobasco at users.sourceforge.net)

---