Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, George Mathews
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef HYDROOGMAP_SIMPLE_OGRAYTRACE_H
00011 #define HYDROOGMAP_SIMPLE_OGRAYTRACE_H
00012 
00013 #include <hydroogmap/ograytracer.h>
00014 #include <hydroportability/sharedlib.h>
00015 
00016 namespace hydroogmap {
00017 
00022 
00023 
00030 class SOEXPORT ILosTracer {
00031 
00032 public:
00033 
00034     virtual ~ILosTracer() {};
00035 
00036     virtual bool isClearCells( int x0, int y0, int x1, int y1, int &endX, int &endY )=0;
00037     virtual bool isClearCells( int x0, int y0, int x1, int y1 )=0;
00038     virtual bool isClearWorld( double x0, double y0, double x1, double y1, double &endX, double &endY )=0;
00039     virtual bool isClearWorld( double x0, double y0, double x1, double y1 )=0;
00040 };
00041 
00048 class SOEXPORT OgLosTracer : public ILosTracer {
00049 
00050 public:
00051 
00052     OgLosTracer( const hydroogmap::OgMap &ogMap,
00053                  unsigned char occupiedThreshold=hydroogmap::CELL_UNKNOWN )
00054         : ogMap_(ogMap),
00055           safeCellTracer_( ogMap, occupiedThreshold ),
00056           safeLosRayTracer_( safeCellTracer_ ),
00057           uncheckedCellTracer_( ogMap, occupiedThreshold ),
00058           uncheckedLosRayTracer_( uncheckedCellTracer_ )
00059         {}
00060 
00067     bool isClearCells( int x0, int y0, int x1, int y1, int &endX, int &endY )
00068         {
00069             bool isLineOfSight = safeLosRayTracer_.trace( x0,y0, x1,y1 );
00070             if ( !isLineOfSight )
00071             {
00072                 endX = safeCellTracer_.lastEndX();
00073                 endY = safeCellTracer_.lastEndY();
00074                 return isLineOfSight;
00075             }
00076             else
00077             {
00078                 endX = x1;
00079                 endY = y1;
00080                 return isLineOfSight;
00081             }
00082         }
00083 
00089     bool isClearCells( int x0, int y0, int x1, int y1 )
00090         {
00091             if ( ogMap_.cellWithinMap(x0,y0) &&
00092                  ogMap_.cellWithinMap(x1,y1) )
00093                 return uncheckedLosRayTracer_.trace( x0,y0, x1,y1 );
00094             else
00095                 return false;
00096         }
00097 
00099     bool isClearWorld( double x0, double y0, double x1, double y1, double &endX, double &endY )
00100         {
00101             int x0c,y0c,x1c,y1c,endXc,endYc;
00102             ogMap_.getCellIndices( x0, y0, x0c, y0c );
00103             ogMap_.getCellIndices( x1, y1, x1c, y1c );
00104             bool clear = isClearCells( x0c, y0c, x1c, y1c, endXc, endYc );
00105             endX = ogMap_.worldXCoord( endXc );
00106             endY = ogMap_.worldYCoord( endYc );
00107             return clear;
00108         }
00109 
00111     bool isClearWorld( double x0, double y0, double x1, double y1 )
00112         {
00113             int x0c,y0c,x1c,y1c;
00114             ogMap_.getCellIndices( x0, y0, x0c, y0c );
00115             ogMap_.getCellIndices( x1, y1, x1c, y1c );
00116             return isClearCells( x0c, y0c, x1c, y1c );
00117         }
00118 
00119 private:
00120 
00121     const OgMap                    &ogMap_;
00122 
00123     // Use this if we're gonna go off the map, and we want to know when
00124     SafeLosCellTracer               safeCellTracer_;
00125     OgRayTracer<SafeLosCellTracer>  safeLosRayTracer_;
00126 
00127     // Use this if we don't care about the exact endpoint, and 
00128     // we know we're gonna stay on the map
00129     UncheckedLosCellTracer              uncheckedCellTracer_;
00130     OgRayTracer<UncheckedLosCellTracer> uncheckedLosRayTracer_;
00131 
00132 };
00133 
00134 
00135 
00136 
00137 }
00138 
00139 
00140 #endif
simpleograytrace.h
