sparseskel.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 HYDROPATHPLAN_SPARSE_SKEL_H
#define HYDROPATHPLAN_SPARSE_SKEL_H

#include <hydropathplan/util.h>
#include <hydroogmap/hydroogmap.h>

namespace hydropathplan {

namespace sparseskel {

class SparseSkelArc;
class ContiguousSparseSkel;
class SparseSkelNode;


class SparseSkel {

public:

    SparseSkel( const hydroogmap::OgMap &ogMap,
                double                   traversabilityThreshhold,
                const Cell2DVector      &skel,
                const FloatMap          &costMap,
                bool                     addExtraNodes,
                double                   extraNodeResolution );

    ~SparseSkel();

    // access
    const std::vector<ContiguousSparseSkel*> &contiguousSkels() const
        { return contiguousSkels_; }
    const hydroogmap::OgMap &ogMap() const { return ogMap_; }
    const hydroogmap::OgMap &extraNodesOgMap() const { return extraNodesOgMap_; }

    int numNodes() const;
    double traversabilityThreshhold() const { return traversabilityThreshhold_; } 

private:

    //
    // 'wps' is the set of waypoints: end-points or junctions of the skel.
    //
    void build( Cell2DList     &skel,
                Cell2DList     &wps,
                const FloatMap &costMap,
                bool            addExtraNodes,
                double          extraNodeResolution );

    // If the map has disjoint sections, we may require more than one
    // separate ContiguousSparseSkel.
    std::vector<ContiguousSparseSkel*> contiguousSkels_;

    // Use this OG map generally.  It should have been pre-grown
    const hydroogmap::OgMap &ogMap_;

    // Use this OG map for adding extra nodes: only add if LOS is very clear.
    hydroogmap::OgMap  extraNodesOgMap_;

    const double traversabilityThreshhold_;
};


//
// If the map has disjoint sections, we may require more than one
// separate ContiguousSparseSkel.
//
// Not intended to be instantiated by anything other than SparseSkel.
//
class ContiguousSparseSkel
{
public:

    //
    // This constructor removes the stuff it uses from the parameters wps and skel.
    //
    ContiguousSparseSkel( SparseSkel     &parent,
                          Cell2DList     &wps,
                          Cell2DList     &skel,
                          const FloatMap &costMap,
                          bool            addExtraNodes,
                          double          extraNodeResolution );
    ~ContiguousSparseSkel();

    // access
    const std::vector<SparseSkelNode*> &nodes() const
        { return nodes_; }

    const hydroogmap::OgMap &ogMap() const { return parent_.ogMap(); }
    double traversabilityThreshhold() const { return parent_.traversabilityThreshhold(); }

    bool isSane() const;

private:

    // This function grows outwards along the skel from 'fromNode',
    // finding and adding all directly-connected neighbours.  It will
    // modify its arguments, adding to 'ungrownCells',
    // and removing from and 'skel'
    void growNode( SparseSkelNode *fromNode,
                   Cell2DList     &ungrownCells,
                   Cell2DList     &wps,
                   Cell2DList     &skel );

    // Search through the skel to find the next node in this
    // direction. findNeighbourNode deletes all visited
    // members of skel.  Visited members of skel get put in
    // cellsEnRoute.
    bool findNeighbourNode( const SparseSkelNode *fromNode,
                            const Cell2D         &startCell,
                            Cell2D               &neighbourPos,
                            Cell2DVector         &cellsEnRoute,
                            Cell2DList           &wps,
                            Cell2DList           &skel );

    SparseSkelNode* findNode( const Cell2D &pos );

    void mergeAdjacentNodes();

    // prior to this, all costs are initialised to -1
    void setArcCosts( const FloatMap &costMap );

    // return true if it's ok to merge.
    bool canMerge( SparseSkelNode *slave, SparseSkelNode *master );
    // replace all references to slave with refs to master.
    void merge( SparseSkelNode *slave, SparseSkelNode *master );

    // Add extra nodes to cover the space a bit better
    // (if this isn't done, the skeleton has trouble accurately estimating the cost
    //  in large open areas where the skeleton just goes through the middle)
    void addExtraNodes( const hydroogmap::OgMap &ogMap, int cellStep );
    void tryAddExtraNode( const Cell2D &cell,
                          double minDistForLinkSq,
                          double maxDistForLinkSq,
                          const hydroogmap::OgMap &ogMap );

    // Returns the node that it arced to (possibly after creating it)
    // Note that cellsEnRoute doesn't include either end-point.
    SparseSkelNode *createArc( SparseSkelNode *fromNode, 
                               Cell2D         &neighbourNodePos,
                               Cell2DVector   &cellsEnRoute );

    std::vector<SparseSkelNode*>  nodes_;
    SparseSkel                   &parent_;
};


//
// Not intended to be instantiated by anything other than SparseSkel.
//
class SparseSkelNode {

public:

    SparseSkelNode( const Cell2D &nodePos )
        : pos(nodePos) {}
    ~SparseSkelNode();

    Cell2D                       pos;
    std::vector<SparseSkelArc*>  arcs;

    // Temporary used for searching
    mutable float                nodeCost;
};


//
// Not intended to be instantiated by anything other than SparseSkel.
//
class SparseSkelArc {

public:

    SparseSkelArc( SparseSkelNode *initToNode, float initCost )
        : toNode(initToNode), cost(initCost) {}

    SparseSkelNode *toNode;
    float           cost;

};

inline float distance( const Cell2D &a, const Cell2D &b )
{
#ifdef __QNX__
    return std::hypot( a.x()-b.x(), a.y()-b.y() );
#else
    return hypot( a.x()-b.x(), a.y()-b.y() );
#endif
}

inline float distanceSq( const Cell2D &a, const Cell2D &b )
{
    return (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y());
}


// // helper function for debugging.
// void printCellList( const Cell2DList &cellList );

}

}

#endif