anominalplannerinformation.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 "anominalplanner.h"
#include <vector>
 
namespace adore
{
    namespace fun
    {
        class ANominalPlannerInformation
        {
        public:
            virtual bool getReferenceIfAvailable(int dim, int der, double t,double s,double ds,double& ref) const = 0;
            virtual double getUB(int dim, int der, double t,double s,double ds) const = 0;
            virtual double getLB(int dim, int der, double t,double s,double ds) const = 0;
            virtual void update(double t0,double s0,double ds0) =0;
        };
 
        template<int N,int D>
        class NominalPlannerInformationSet:public ANominalPlannerInformation
        {
        private:
            typedef std::vector<ANominalConstraint*> TCset;
            typedef std::vector<ANominalReference*> TRset;
            typedef std::vector<ANominalPlannerInformation*> TIset;
            TCset bound_[D*N*2];
            TRset ref_[D*N];
            TIset subsets_;
            double unbound_;
 
        public:
            NominalPlannerInformationSet()
            {
                unbound_ = 1e5;
            }
            void add(ANominalReference* r)
            {
                ref_[r->getDimension()*N+r->getDerivative()].push_back(r);
            }
            void add(ANominalConstraint* c)
            {
                int id = c->getDirection()==ANominalConstraint::UB?0:1;
                bound_[c->getDimension()*N*2 + c->getDerivative()*2 + id].push_back(c);
            }
            bool remove(ANominalConstraint* c)
            {
                int id = c->getDirection()==ANominalConstraint::UB?0:1;
                TCset::iterator it;
                it = std::find(
                    bound_[c->getDimension()*N*2 + c->getDerivative()*2 + id].begin(),
                    bound_[c->getDimension()*N*2 + c->getDerivative()*2 + id].end(),
                    c);
                if (it != bound_[c->getDimension()*N*2 + c->getDerivative()*2 + id].end())
                {
                    bound_[c->getDimension()*N*2 + c->getDerivative()*2 + id].erase(it);
                    return true;
                }
                return false;
            }
            bool remove(ANominalReference* r)
            {
                TRset::iterator it;
                it = std::find(
                    ref_[r->getDimension()*N + r->getDerivative()].begin(),
                    ref_[r->getDimension()*N + r->getDerivative()].end(),
                    r);
                if (it != ref_[r->getDimension()*N + r->getDerivative()].end())
                {
                    ref_[r->getDimension()*N + r->getDerivative()].erase(it);
                    return true;
                }
                return false;
            }
            void add(ANominalPlannerInformation* value)
            {
                subsets_.push_back(value);
            }
 
            bool remove(ANominalPlannerInformation* value)
            {
                auto it = std::find(subsets_.begin(), subsets_.end(), value);
                if (it != subsets_.end())
                {
                    subsets_.erase(it);
                    return true;
                }
                return false;
            }
        public:
            virtual bool getReferenceIfAvailable(int dim, int der, double t,double s,double ds,double& ref) const  override
            {
                for(ANominalReference* r:ref_[dim*N+der])
                {
                    if(r->getValueIfAvailable(t,s,ds,ref))return true;
                }
                for(ANominalPlannerInformation* subset:subsets_)
                {
                    if(subset->getReferenceIfAvailable(dim,der,t,s,ds,ref))return true;
                }
                return false;
            }
            virtual double getUB(int dim, int der, double t,double s,double ds) const  override
            {
                double value = unbound_;
                for(ANominalConstraint* c:bound_[dim*N*2 + der*2 + 0])//walk through the own constraints
                {
                    value = (std::min)(value,c->getValue(t,s,ds));
                }
                for(ANominalPlannerInformation* subset:subsets_)//query the subsets
                {
                    value = (std::min)(value,subset->getUB(dim,der,t,s,ds));
                }
                return value;
            }
            virtual double getLB(int dim, int der, double t,double s,double ds) const  override
            {
                double value = -unbound_;
                for(ANominalConstraint* c:bound_[dim*N*2 + der*2 + 1])//walk through the own constraints
                {
                    value = (std::max)(value,c->getValue(t,s,ds));
                }
                for(ANominalPlannerInformation* subset:subsets_)//query the subsets
                {
                    value = (std::max)(value,subset->getLB(dim,der,t,s,ds));
                }
                return value;
            }
            virtual void update(double t0,double s0,double ds0) override
            {
                for(auto it=subsets_.begin();it!=subsets_.end();it++)(*it)->update(t0,s0,ds0);
                for(int i=0;i<D*N;i++)for(auto it=ref_[i].begin();it!=ref_[i].end();it++)(*it)->update(t0,s0,ds0);
                for(int i=0;i<D*N*2;i++)for(auto it=bound_[i].begin();it!=bound_[i].end();it++)(*it)->update(t0,s0,ds0);
            }
        };
    }
}