lanepositionedobjectset.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 API and implementation
 ********************************************************************************/
 
 
#pragma once
#include <adore/env/borderbased/border.h>
#include <adore/env/borderbased/borderset.h>
#include <adore/env/borderbased/alanepositionedobject.h>
#include <vector>
#include <unordered_map>
 
namespace adore
{
    namespace env
    {
        namespace BorderBased
        {
            class LanePositionedObjectSet
            {
            
 
            public:
                typedef ALanePositionedObject object;
                typedef std::unordered_multimap<BorderID,object*, BorderIDHasher> BorderID2Object;
                typedef BorderID2Object::iterator ObjectIterator;
                typedef std::pair<ObjectIterator,ObjectIterator> ObjectIteratorPair;
            private:
                BorderID2Object borderId2Object; 
                bool m_isOwner; 
            public:
                LanePositionedObjectSet()
                {
                    m_isOwner = true;
                }
                virtual ~LanePositionedObjectSet()
                {
                    clear();
                }
                void setIsOwner(bool isOwner)
                {
                    m_isOwner = isOwner;
                }
                void clear()
                {
                    if(m_isOwner)
                    {
                        for(auto it = borderId2Object.begin();it!=borderId2Object.end();it++)
                        {
                            delete it->second;
                        }
                    }
                    borderId2Object.clear();
                }
                void discard_distant_objects(double x, double y, double distance = 100.0)
                {
                    for (auto it = getAllObjects().first; it != getAllObjects().second; )
                    {   
                        auto checkCoord = Coordinate(x,y,0.0);
 
                        if(it->first.m_first.distance(checkCoord)>=distance
                            && it->first.m_last.distance(checkCoord) >= distance)
                        //if (!it->first.m_first.isInArea(x-distance,x+distance,y-distance, y+distance))
                        {
                            delete it->second;
                            it=borderId2Object.erase(it);
                            continue;
                        }
                        it++;
                    }
                }
                template <typename T>
                void discard_distant_objects(double x, double y, std::vector<T> &removedObjects, double distance = 100.0)
                {
                    for (auto it = getAllObjects().first; it != getAllObjects().second; )
                    {   
                        auto checkCoord = Coordinate(x,y,0.0);
 
                        if(it->first.m_first.distance(checkCoord)>=distance
                            && it->first.m_last.distance(checkCoord) >= distance)
                        //if (!it->first.m_first.isInArea(x-distance,x+distance,y-distance, y+distance))
                        {
                            removedObjects.push_back(*(dynamic_cast<T*>((*it).second)));
 
                            delete it->second;
                            it=borderId2Object.erase(it);
                            continue;
                        }
                        it++;
                    }
                }
                void insert_object(object* obj,bool remove_duplicates=false,double precision=0.5)
                {
                    if(remove_duplicates)
                        erase_object(obj->getLanePosition(),precision);
                    
                    borderId2Object.insert(std::make_pair(obj->getLanePosition().m_rightID,obj));
                }
                void erase_object(const LanePosition& position,double precision=0.5)
                {
                    for(auto it = borderId2Object.equal_range(position.m_rightID);it.first!=it.second;)
                    {
                        if((std::abs)(it.first->second->getLanePosition().m_progress-position.m_progress)<precision)
                        {
                            if(m_isOwner)delete it.first->second;
                            it.first = borderId2Object.erase(it.first);
                            continue;
                        }
                        it.first++;
                    }
                }
                void eraseObjectsBorderBased(const std::vector<Border*> borderSet)
                {
                    for(auto it = borderSet.begin();it!=borderSet.end();it++)//borderSet
                    {
                        for(auto it2 = borderId2Object.equal_range((*it)->m_id);it2.first!=it2.second;)//borderid2object
                        {
                            if(m_isOwner)delete it2.first->second;
                            it2.first = borderId2Object.erase(it2.first);
                        }
                    }
                }
                void eraseObjectsBorderBased(const std::vector<BorderID> borderIDSet)
                {
                    for(auto it = borderIDSet.begin();it!=borderIDSet.end();it++)//borderIDVector
                    {
                        for(auto it2 = borderId2Object.equal_range(*it);it2.first!=it2.second;)//borderid2object
                        {
                            if(m_isOwner)delete it2.first->second;
                            it2.first = borderId2Object.erase(it2.first);
                        }
                    }
                }
                bool hasObjects(const BorderID& borderID)
                {
                    if(borderId2Object.size() == 0)
                        return false;
 
                    return borderId2Object.count(borderID)>0;
                }
                ObjectIteratorPair getAllObjects()
                {
                    return ObjectIteratorPair(borderId2Object.begin(),borderId2Object.end());
                }
                ObjectIteratorPair getObjects(const BorderID& borderID)
                {
                    return borderId2Object.equal_range(borderID);
                }
                std::vector<object*> getObjects(const LanePosition& position,double precision=0.5)
                {
                    std::vector<object*> result;
                    for( auto itpair=getObjects(position.m_rightID);itpair.first!=itpair.second;itpair.first++ )
                    {
                        if((std::abs)(itpair.first->second->getLanePosition().m_progress-position.m_progress)<precision)
                        {
                            result.push_back(itpair.first->second);
                        }
                    }
                    return result;
                }
                bool hasObjects(const LanePosition& position,double precision=0.5)
                {
                    for( auto itpair=getObjects(position.m_rightID);itpair.first!=itpair.second;itpair.first++ )
                    {
                        if((std::abs)(itpair.first->second->getLanePosition().m_progress-position.m_progress)<precision)
                        {
                            return true;
                        }
                    }
                    return false;
                }
                void erase_objectsWithUnknownBordersOutsideRadius(BorderSet* borderSet,Coordinate center,double radius)
                {
                    for( auto it = borderId2Object.begin();it!=borderId2Object.end(); )
                    {
                        auto bid = it->first;
                        if(!borderSet->hasBorder(bid))
                        {
                            if(bid.m_first.distance(center)>radius
                             && bid.m_last.distance(center)>radius)
                            {
                                if(m_isOwner)delete it->second;
                                it = borderId2Object.erase(it);
                                continue;
                            }
                        }
                        it++;
                    }
                }
            };
 
        }
    }
}