Grid.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#ifndef OMPL_DATASTRUCTURES_GRID_
#define OMPL_DATASTRUCTURES_GRID_

#include <vector>
#include <iostream>
#include <cstdlib>
#include <boost/unordered_map.hpp>
#include <algorithm>

namespace ompl
{

    template <typename _T>
    class Grid
    {
    public:

        typedef std::vector<int> Coord;

        struct Cell
        {
            _T                  data;

            Coord               coord;

            Cell(void)
            {
            }

            virtual ~Cell(void)
            {
            }
        };

        typedef std::vector<Cell*> CellArray;


        explicit
        Grid(unsigned int dimension)
        {
            setDimension(dimension);
        }

        virtual ~Grid(void)
        {
            freeMemory();
        }

        virtual void clear(void)
        {
            freeMemory();
        }

        unsigned int getDimension(void) const
        {
            return dimension_;
        }

        void setDimension(unsigned int dimension)
        {
            if (!empty())
                throw;
            dimension_ = dimension;
            maxNeighbors_ = 2 * dimension_;
        }

        bool has(const Coord &coord) const
        {
            return getCell(coord) != NULL;
        }

        Cell* getCell(const Coord &coord) const
        {
            iterator pos = hash_.find(const_cast<Coord*>(&coord));
            Cell *c = (pos != hash_.end()) ? pos->second : NULL;
            return c;
        }

        void    neighbors(const Cell* cell, CellArray& list) const
        {
            Coord test = cell->coord;
            neighbors(test, list);
        }

        void    neighbors(const Coord& coord, CellArray& list) const
        {
            Coord test = coord;
            neighbors(test, list);
        }

        void    neighbors(Coord& coord, CellArray& list) const
        {
            list.reserve(list.size() + maxNeighbors_);

            for (int i = dimension_ - 1 ; i >= 0 ; --i)
            {
                coord[i]--;

                iterator pos = hash_.find(&coord);
                Cell *cell = (pos != hash_.end()) ? pos->second : NULL;

                if (cell)
                    list.push_back(cell);
                coord[i] += 2;

                pos = hash_.find(&coord);
                cell = (pos != hash_.end()) ? pos->second : NULL;

                if (cell)
                    list.push_back(cell);
                coord[i]--;
            }
        }

        std::vector< std::vector<Cell*> > components(void) const
        {
            typedef boost::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash;
            typedef typename ComponentHash::iterator CHit;

            int components = 0;
            ComponentHash ch;
            std::vector< std::vector<Cell*> > res;

            for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
            {
                Cell *c0 = i->second;
                CHit pos = ch.find(&c0->coord);
                int comp = (pos != ch.end()) ? pos->second : -1;

                if (comp < 0)
                {
                    res.resize(res.size() + 1);
                    std::vector<Cell*> &q = res.back();
                    q.push_back(c0);
                    std::size_t index = 0;
                    while (index < q.size())
                    {
                        Cell *c = q[index++];
                        pos = ch.find(&c->coord);
                        comp = (pos != ch.end()) ? pos->second : -1;

                        if (comp < 0)
                        {
                            ch.insert(std::make_pair(&c->coord, components));
                            std::vector< Cell* > nbh;
                            neighbors(c, nbh);
                            for (unsigned int j = 0 ; j < nbh.size() ; ++j)
                            {
                                pos = ch.find(&nbh[j]->coord);
                                comp = (pos != ch.end()) ? pos->second : -1;
                                if (comp < 0)
                                    q.push_back(nbh[j]);
                            }
                        }
                        else
                        {
                            --index;
                            q.erase(q.begin() + index);
                        }
                    }
                    ++components;
                }
            }
            std::sort(res.begin(), res.end(), SortComponents());
            return res;
        }

        virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
        {
            Cell *cell = new Cell();
            cell->coord = coord;
            if (nbh)
                neighbors(cell->coord, *nbh);
            return cell;
        }

        virtual bool remove(Cell *cell)
        {
            if (cell)
            {
                typename CoordHash::iterator pos = hash_.find(&cell->coord);
                if (pos != hash_.end())
                {
                    hash_.erase(pos);
                    return true;
                }
            }
            return false;
        }

        virtual void add(Cell *cell)
        {
            hash_.insert(std::make_pair(&cell->coord, cell));
        }

        virtual void destroyCell(Cell *cell) const
        {
            delete cell;
        }

        void getContent(std::vector<_T> &content) const
        {
            for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
                content.push_back(i->second->data);
        }

        void getCoordinates(std::vector<Coord*> &coords) const
        {
            for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
                coords.push_back(i->first);
        }

        void getCells(CellArray &cells) const
        {
            for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
                cells.push_back(i->second);
        }

        void printCoord(Coord& coord, std::ostream &out = std::cout) const
        {
            out << "[ ";
            for (unsigned int i = 0 ; i < dimension_ ; ++i)
                out << coord[i] << " ";
            out << "]" << std::endl;
        }

        bool empty(void) const
        {
            return hash_.empty();
        }

        unsigned int size(void) const
        {
            return hash_.size();
        }

        virtual void status(std::ostream &out = std::cout) const
        {
            out << size() << " total cells " << std::endl;
            const std::vector< std::vector<Cell*> > &comp = components();
            out << comp.size() << " connected components: ";
            for (std::size_t i = 0 ; i < comp.size() ; ++i)
                out << comp[i].size() << " ";
            out << std::endl;
        }

    protected:

        void freeMemory(void)
        {
            CellArray content;
            getCells(content);
            hash_.clear();

            for (unsigned int i = 0 ; i < content.size() ; ++i)
                delete content[i];
        }

        struct HashFunCoordPtr
        {
            std::size_t operator()(const Coord* const s) const
            {
                unsigned long h = 0;
                for (int i = s->size() - 1; i >= 0; --i)
                {
                    int high = h & 0xf8000000;
                    h = h << 5;
                    h = h ^ (high >> 27);
                    h = h ^ s->at(i);
                }
                return (std::size_t) h;
            }
        };


        struct EqualCoordPtr
        {
            bool operator()(const Coord* const c1, const Coord* const c2) const
            {
                return *c1 == *c2;
            }
        };

        typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash;

        struct SortComponents
        {
            bool operator()(const std::vector<Cell*> &a, const std::vector<Cell*> &b) const
            {
                return a.size() > b.size();
            }
        };

    public:

        typedef typename CoordHash::const_iterator iterator;

        iterator begin(void) const
        {
            return hash_.begin();
        }

        iterator end(void) const
        {
            return hash_.end();
        }

    protected:

        unsigned int     dimension_;

        unsigned int     maxNeighbors_;

        CoordHash        hash_;
    };
}

#endif

---

ompl
Author(s): Ioan Sucan/isucan@rice.edu, Mark Moll/mmoll@rice.edu, Lydia Kavraki/kavraki@rice.edu
autogenerated on Fri Jan 11 09:33:19 2013