util.h

/*
 * Orca-Robotics Project: Components for robotics 
 *               http://orca-robotics.sf.net/
 * Copyright (c) 2004-2009 Alex Brooks, Alexei Makarenko, Tobias Kaupp
 *
 * This copy of Orca is licensed to you under the terms described in
 * the LICENSE file included in this distribution.
 *
 */
#ifndef HYDRO_PATHPLAN_UTIL_H
#define HYDRO_PATHPLAN_UTIL_H

#include <vector>
#include <hydroogmap/hydroogmap.h>
#include "cell2d.h"
#include "typemap.h"

namespace hydropathplan
{

    class CostEvaluator {
    public:
        virtual ~CostEvaluator() {}
        
        // returns the cost of moving through a cell whose 4-distance to the nearest obstacle is distInMetres.
        virtual double costAtDistFromObstacle( double distInMetres ) const=0;
    };

    class DefaultCostEvaluator : public CostEvaluator {
    public:
        double costAtDistFromObstacle( double distInMetres ) const
            { if ( distInMetres > 0 ) return 1; else return NAN; }
    };

// The 'util' namespace contains stuff that's not intended for use outside of this library.
namespace util {

    bool isTraversable( const hydroogmap::OgMap &ogMap,
                        const int indX,
                        const int indY,
                        const float traversabilityThreshhold);

    inline bool isTraversable( const hydroogmap::OgMap &ogMap,
                               const Cell2D            &cell,
                               const float              traversabilityThreshhold)
    { return isTraversable(ogMap,cell.x(),cell.y(),traversabilityThreshhold); }

    bool isTraversableNeighbors( const hydroogmap::OgMap &ogMap,
                                 const int indX,
                                 const int indY,
                                 const float traversabilityThreshhold );

    bool isTraversableAll8Neighbors( const hydroogmap::OgMap &ogMap,
                                 const int indX,
                                 const int indY,
                                 const float traversabilityThreshhold );

    bool losExists( const hydroogmap::OgMap &ogMap,
                    double traversabilityThreshhold,
                    const Cell2D &c1,
                    const Cell2D &c2 );

    int robotDiameterInCells( const hydroogmap::OgMap &ogMap, const double robotDiameterMetres );

    //  TODO: AlexB: Why grow by the diameter rather than the radius??
    void growObstaclesOgMap( hydroogmap::OgMap &ogMap,
                             const double       traversabilityThreshhold,
                             const int          robotDiameterCells );

    void optimizePath( const hydroogmap::OgMap &ogMap,
                       double                   traversabilityThreshhold,
                       const Cell2DVector      &origPath,
                       Cell2DVector            &optimisedPath );

    float straightLineCost( const Cell2D   &fromCell,
                            const Cell2D   &toCell,
                            const FloatMap &costMap );
    
}

}

#endif