precedence.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 <adore/mad/com_patterns.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <string>
#include <iostream>
#include <fstream>
 
namespace adore
{
    namespace env
    {
        struct PriorityRoute
        {
            public:
            typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian> boost_point;
            typedef boost::geometry::model::box<boost_point> boost_box;
            adoreMatrix<double,3,1> from_;
            adoreMatrix<double,3,1> to_;
            bool coordinates_in_UTM_;
            PriorityRoute()
            {
                set(0.0,0.0,0.0,0.0,0.0,0.0);
                coordinates_in_UTM_ = true;
            }
            void set(double f1,double f2,double f3,double t1,double t2,double t3)
            {
                from_(0)=f1;
                from_(1)=f2;
                from_(2)=f3;
                to_(0)=t1;
                to_(1)=t2;
                to_(2)=t3;
            }
            void set(const std::string& s, bool coordinates_in_UTM=true)
            {
                std::stringstream is;
                double f1,f2,f3,t1,t2,t3;
                char sep;
                is<<s;
                is>> f1 >> sep >> f2 >> sep >> f3 >> sep >> t1 >> sep >> t2 >> sep >> t3;
                set(f1,f2,f3,t1,t2,t3);
                coordinates_in_UTM_ = coordinates_in_UTM;
                std::cout<<s<<std::endl;
            }
            boost_box getBoostBox()
            {
                const double min0 = (std::min)(from_(0),to_(0));
                const double min1 = (std::min)(from_(1),to_(1));
                const double min2 = (std::min)(from_(2),to_(2));
                const double max0 = (std::max)(from_(0),to_(0));
                const double max1 = (std::max)(from_(1),to_(1));
                const double max2 = (std::max)(from_(2),to_(2));
                return boost_box(boost_point(min0,min1,min2),boost_point(max0,max1,max2));
            }
            bool equals(const PriorityRoute& other)const
            {
                return  this->from_(0)==other.from_(0)
                &&      this->from_(1)==other.from_(1)
                &&      this->from_(2)==other.from_(2)
                &&      this->to_(0)==other.to_(0)
                &&      this->to_(1)==other.to_(1)
                &&      this->to_(2)==other.to_(2);                
            }
        };
        struct PrecedenceRule
        {
            public:
            PriorityRoute low_;
            PriorityRoute high_;
            bool unary_;
            PrecedenceRule():unary_(true){}
 
            void set(const std::string& s,bool coordinates_in_UTM=true)
            {
                std::size_t pos = s.find(">");
                if(pos!=std::string::npos)
                {
                    high_.set(s.substr(0,pos),coordinates_in_UTM);
                    low_.set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
                    unary_=false;
                }
                else
                {
                    pos = s.find("<");
                    if(pos!=std::string::npos)
                    {
                        low_.set(s.substr(0,pos),coordinates_in_UTM);
                        high_.set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
                        unary_=false;
                    }
                    else
                    {
                        high_.set(s,coordinates_in_UTM);
                        unary_=true;
                    }
                    
                }
            }
            bool equals(const PrecedenceRule& other)const
            {
                if(this->unary_!=other.unary_)return false;
                if(!this->high_.equals(other.high_))return false;
                if(this->unary_ && !this->low_.equals(other.low_))return false;
                return true;
            }
 
            PriorityRoute::boost_box getBoostBox()
            {
                if(unary_)return high_.getBoostBox();
                auto l = low_.getBoostBox();
                auto h = high_.getBoostBox();
                l.min_corner().get<0>();
                const double min0 = (std::min)(l.min_corner().get<0>(),h.min_corner().get<0>());
                const double min1 = (std::min)(l.min_corner().get<1>(),h.min_corner().get<1>());
                const double min2 = (std::min)(l.min_corner().get<2>(),h.min_corner().get<2>());
                const double max0 = (std::max)(l.max_corner().get<0>(),h.max_corner().get<0>());
                const double max1 = (std::max)(l.max_corner().get<1>(),h.max_corner().get<1>());
                const double max2 = (std::max)(l.max_corner().get<2>(),h.max_corner().get<2>());
                return PriorityRoute::boost_box(
                            PriorityRoute::boost_point(min0,min1,min2),
                            PriorityRoute::boost_point(max0,max1,max2)
                            );
            }
        };
 
        class PrecedenceSet
        {
            private:
            adore::mad::AFeed<PrecedenceRule>* ruleFeed_;
 
            public:
            PrecedenceSet(){}
            PrecedenceSet(adore::mad::AFeed<PrecedenceRule>* ruleFeed)
            {
               ruleFeed_ = ruleFeed;
            }
            void update(double radius, double x, double y)
            {
                while(ruleFeed_->hasNext())
                {
                    PrecedenceRule r;
                    ruleFeed_->getNext(r);
                    insertRule(r);
                }
                refocus(x-radius,y-radius,x+radius,y+radius); 
            }
            template<typename value_type,typename Tfirst>
            struct my_equal 
            { 
                typedef bool result_type; 
                result_type operator() (value_type const& v1, value_type const& v2) const 
                { 
                    return boost::geometry::equals<Tfirst,Tfirst>(v1.first, v2.first) && v1.second == v2.second;
                } 
            }; 
            typedef std::pair<PriorityRoute::boost_box,PrecedenceRule*> idxRegion2Precedence;
            typedef boost::geometry::index::rtree<  idxRegion2Precedence,
                                                    boost::geometry::index::quadratic<16>,
                                                    boost::geometry::index::indexable<idxRegion2Precedence>,
                                                    my_equal<idxRegion2Precedence,PriorityRoute::boost_box>
                                                    > Region2PrecedenceRT;
            template<typename T1, typename T2>
            struct itpair
            {
                T1 first;
                T2 second;
                itpair(T1 first,T2 second):first(first),second(second){}
                T1& current(){return first;}
                T2& end(){return second;}
            };
            typedef itpair<Region2PrecedenceRT::const_query_iterator,Region2PrecedenceRT::const_query_iterator> itRegion2PrecedenceRT;
 
            private:
            Region2PrecedenceRT precedenceRT_;
            public:
 
            bool readFile(const std::string& filename)
            {
                std::string line;
                std::ifstream file(filename);
                if(!file.is_open())
                {
                    std::cout<<"failed to open file\n";
                    return false;
                }
                while(std::getline(file,line))
                {
                    if (line.empty() || line.at(0) == '#')
                    {
                        continue;
                    }
                    auto rule = parseRule(line);//@TODO: parse coordinate format from file
                    std::cout<<precedenceRT_.size()<<": ";
                    std::cout<<
                        rule->high_.from_(0)<<","<<
                        rule->high_.from_(1)<<","<<
                        rule->high_.from_(2)<<";"<<
                        rule->high_.to_(0)<<","<<
                        rule->high_.to_(1)<<","<<
                        rule->high_.to_(2);
                    if(!rule->unary_)
                    {
                    std::cout<<
                        " > "<<
                        rule->low_.from_(0)<<","<<
                        rule->low_.from_(1)<<","<<
                        rule->low_.from_(2)<<";"<<
                        rule->low_.to_(0)<<","<<
                        rule->low_.to_(1)<<","<<
                        rule->low_.to_(2);
                    }
                    std::cout<<"\n";
                }
                return true;
            }
 
            PrecedenceRule* parseRule(const std::string& s,bool use_UTM_coordinates = true)
            {
                PrecedenceRule* myrule = new PrecedenceRule();
                myrule->set(s,use_UTM_coordinates);
                precedenceRT_.insert(std::make_pair(myrule->getBoostBox(),myrule));
                return myrule;
            }
            void insertRule(const PrecedenceRule& rule)
            {
                PrecedenceRule* myrule = new PrecedenceRule();
                *myrule=rule;
                precedenceRT_.insert(std::make_pair(myrule->getBoostBox(),myrule));
            }
            void init(PrecedenceSet* other)
            {
                std::cout<<"copying...";
                for(auto const& pair : other->precedenceRT_)
                {
                    std::cout<<"+";
                    auto rule = pair.second;
                    insertRule(*rule);
                }
            }
            bool contains(const PrecedenceRule& rule)
            {
                for(auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
                                                PriorityRoute::boost_box(
                                                PriorityRoute::boost_point(
                                                    rule.high_.from_(0),
                                                    rule.high_.from_(1),
                                                    rule.high_.from_(2)
                                                ),
                                                PriorityRoute::boost_point(
                                                    rule.high_.from_(0),
                                                    rule.high_.from_(1),
                                                    rule.high_.from_(2)
                                                ))));
                    it!=precedenceRT_.qend();
                    it++)
                    {
                        if(it->second->equals(rule))return true;
                    }
                return false;                
            }
            void eraseRule(PrecedenceRule* rule)
            {
                precedenceRT_.remove(std::make_pair(rule->getBoostBox(),rule));
                delete rule;
            }
            itRegion2PrecedenceRT getRulesInRegion(double x0,double y0,double x1,double y1) const
            {
                static const double guard = 1.0e99;
                auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
                                            PriorityRoute::boost_box(   
                                                PriorityRoute::boost_point(x0,y0,-guard),
                                                PriorityRoute::boost_point(x1,y1,+guard) 
                                            )
                                        ));
                return itRegion2PrecedenceRT(it,precedenceRT_.qend());
            }
            itRegion2PrecedenceRT getAllRulesIt() const
            {
                static const double guard = 1.0e99;
                auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
                                            PriorityRoute::boost_box(   
                                                PriorityRoute::boost_point(-guard,-guard,-guard),
                                                PriorityRoute::boost_point(+guard,+guard,+guard) 
                                            )
                                        ));
                return itRegion2PrecedenceRT(it,precedenceRT_.qend());
            }
            
            Region2PrecedenceRT* getAllRules()
            {
                return &precedenceRT_;
            }
            itRegion2PrecedenceRT getRulesOutsideRegion(double x0,double y0,double x1,double y1)
            {
                static const double guard = 1.0e99;
                auto it = precedenceRT_.qbegin(boost::geometry::index::disjoint(
                                            PriorityRoute::boost_box(   
                                                PriorityRoute::boost_point(x0,y0,-guard),
                                                PriorityRoute::boost_point(x1,y1,+guard) 
                                            )
                                        ));
                return itRegion2PrecedenceRT(it,precedenceRT_.qend());
            }
            void refocus(double x0,double y0,double x1,double y1)
            {
                std::vector<PrecedenceRule*> removeset;
                for(auto it = getRulesOutsideRegion(x0,y0,x1,y1);it.current()!=it.end();it.current()++)
                {
                    removeset.push_back(it.current()->second);
                }
                for(auto it: removeset)eraseRule(it);
            }
 
            
        };
    }
}