GenerateNav.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __ai_GenerateNav_h__
#define __ai_GenerateNav_h__

#include "signal/signal.h"
#include "math/math.h"
#include "datatool/datatool.h"
#include "shape/shape.h"
#include "mobility/mobilityAS.h"
#include "StepCostI.h"
#include "aiAS.h"

namespace fe
{

/** Generate a navigation field, which is a vector field of directions to
    get to a target destination point.

    @copydoc GenerateNav_info
    */
class FE_DL_EXPORT GenerateNav :
    public Initialize<GenerateNav>,
    virtual public HandlerI,
    virtual public Config
{
    public:
                GenerateNav(void);
virtual         ~GenerateNav(void);

        void    initialize(void);

                // AS HandlerI
virtual void    handle( Record &r_sig);

    private:
        AsNav           m_asNav;
        AsNavDebug      m_asNavDebug;
};

template<typename P>
void wavefrontPotential(    Continuum<P> &a_potential,
                            sp<StepCostI> a_costFn,
                            const SpatialVector &a_target,
                            const P &a_max, const P &a_min)
{
    typedef typename Continuum<P>::t_index t_index;
    t_index target_index;
    a_potential.index(target_index, a_target);

    a_potential.unit().safe(target_index);

fprintf(stderr, "target %f %f %f   index %d %d %d\n", a_target[0], a_target[1], a_target[2], target_index[0], target_index[1], target_index[2]);

    std::list<t_index> checklist;
    checklist.push_back(target_index);

    a_potential.accessUnit().set(a_max);
    a_potential.accessUnit().set(target_index, a_min);

    while(checklist.begin() != checklist.end())
    {
        t_index i_current;
        i_current = checklist.front();
        checklist.pop_front();

        std::list<t_index> neighbors;

        a_potential.accessUnit().addNeighbors(neighbors, i_current);

        bool dup = false;
        for(typename std::list<t_index>::iterator
            i_neighbor = neighbors.begin();
            i_neighbor != neighbors.end(); i_neighbor++)
        {
            typename std::list<t_index>::iterator i_other = i_neighbor;
            i_other++;
            while(i_other != neighbors.end())
            {
                if(*i_neighbor == *i_other)
                {
                    dup = true;
                }
                i_other++;
            }
            P dp;
            SpatialVector current_location;
            a_potential.location(current_location, i_current);
            SpatialVector neighbor_location;
            a_potential.location(neighbor_location, *i_neighbor);
            dp = a_costFn->cost(neighbor_location, current_location);
            P new_value = a_potential.unit()[i_current] + dp;
            if(new_value < a_potential.unit()[*i_neighbor])
            {
                a_potential.accessUnit().set(*i_neighbor, new_value);
                checklist.push_back(*i_neighbor);
            }
        }
        if(dup){ feX("duplicate\n");}
    }
}


} /* namespace */

#endif /* __ai_GenerateNav_h__ */