localboxset.h

Go to the documentation of this file.

/********************************************************************************
 * Copyright (C) 2017-2020 German Aerospace Center (DLR). 
 * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
 *
 * This program and the accompanying materials are made available under the 
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0.
 *
 * SPDX-License-Identifier: EPL-2.0 
 *
 * Contributors: 
 *    Daniel Heß - initial implementation and API
 ********************************************************************************/
 
 
#pragma once
#include <adore/mad/adoremath.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
 
namespace adore
{
    namespace env
    {
        template<typename TObject,typename TBoxGen,typename TComparator>
        class LocalBoxSet
        {
            public:
                typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian> Tboost_point;
                typedef boost::geometry::model::box<Tboost_point> Tboost_box;
                typedef std::pair<Tboost_box,TObject*> TBoxPointerPair;
                struct EqualFunctor
                { 
                    TComparator compare_;
                    bool operator() (TBoxPointerPair const& v1, TBoxPointerPair const& v2) const 
                    { 
                        return boost::geometry::equals<Tboost_box,Tboost_box>(v1.first, v2.first)&&(v1.second == v2.second);
                    }
                };
                typedef boost::geometry::index::rtree<  TBoxPointerPair,
                                                        boost::geometry::index::quadratic<16>,
                                                        boost::geometry::index::indexable<TBoxPointerPair>,
                                                        EqualFunctor > Trtree;
                typedef typename Trtree::const_query_iterator Trtreeit;
                struct itpair
                {
                    Trtreeit first,second;
                    itpair(Trtreeit first,Trtreeit second):first(first),second(second){}
                    Trtreeit& current(){return first;}
                    Trtreeit& end(){return second;}
                };
            private:
                Trtree data_;
                TBoxGen boxgen_;
                TComparator compare_;
            public:
                void insert(const TObject& object)
                {
                    TObject* old = find(object);
                    if(old !=0)
                    {
                        remove(old);
                    }
                    TObject* copy = new TObject(object);
                    Tboost_box box  = boxgen_(copy);
                    TBoxPointerPair pair = std::make_pair(box,copy);
                    data_.insert(pair);
                }
                TObject* find(const TObject& object)
                {
                    Tboost_box box = boxgen_(&object);
                    for(auto it = data_.qbegin(boost::geometry::index::intersects(box));
                             it!= data_.qend();
                             it++)
                    {
                        if(compare_(it->second,&object))return it->second;
                    }
                    return 0;
                }
                bool contains(const TObject& object)
                {
                    return find(object)!=0;                
                }
                void remove(TObject* object)
                {
                    data_.remove(std::make_pair(boxgen_(object),object));
                    delete object;
                }
                int size()const{return data_.size();}
 
                itpair getObjectsInRegion(double x0,double y0,double x1,double y1)
                {
                    static const double guard = 1.0e99;
                    auto it = data_.qbegin(boost::geometry::index::intersects(
                                                Tboost_box( 
                                                    Tboost_point(x0,y0,-guard),
                                                    Tboost_point(x1,y1,+guard) 
                                                )
                                            ));
                    return itpair(it,data_.qend());
                }
                itpair getObjectsOutsideRegion(double x0,double y0,double x1,double y1)
                {
                    static const double guard = 1.0e99;
                    auto it = data_.qbegin(boost::geometry::index::disjoint(
                                                Tboost_box( 
                                                    Tboost_point(x0,y0,-guard),
                                                    Tboost_point(x1,y1,+guard) 
                                                )
                                            ));
                    return itpair(it,data_.qend());
                }
                void refocus(double x0,double y0,double x1,double y1)
                {
                    std::vector<TObject*> removeset;
                    for(auto itp = getObjectsOutsideRegion(x0,y0,x1,y1);
                        itp.current()!=itp.end();
                        itp.current()++)
                    {
                        removeset.push_back(itp.current()->second);
                    }
                    for(auto obj: removeset)remove(obj);
                }
        };
    }
}