From 624c92e934b761ea2f253a396abb48c498c50bba Mon Sep 17 00:00:00 2001 From: nojhan Date: Thu, 25 Jun 2020 10:21:19 +0200 Subject: [PATCH] first version --- README.md | 20 +++ cpp/CMakeLists.txt | 35 +++++ cpp/code.h | 322 +++++++++++++++++++++++++++++++++++++++++++ cpp/crtp.cpp | 151 ++++++++++++++++++++ cpp/functional.cpp | 112 +++++++++++++++ cpp/policies.cpp | 162 ++++++++++++++++++++++ cpp/strategy.cpp | 160 +++++++++++++++++++++ python/code.py | 206 +++++++++++++++++++++++++++ python/functional.py | 64 +++++++++ python/strategy.py | 101 ++++++++++++++ 10 files changed, 1333 insertions(+) create mode 100644 README.md create mode 100644 cpp/CMakeLists.txt create mode 100644 cpp/code.h create mode 100644 cpp/crtp.cpp create mode 100644 cpp/functional.cpp create mode 100644 cpp/policies.cpp create mode 100644 cpp/strategy.cpp create mode 100644 python/code.py create mode 100644 python/functional.py create mode 100644 python/strategy.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..b9433fb --- /dev/null +++ b/README.md @@ -0,0 +1,20 @@ +Educational demonstration of several design patterns which may be useful in algorithmics, in several languages. + +The algorithms are simple motion planning methods +and the patterns are centered on composition. +The code demonstrates how to *compose* either a Dijkstra algorithm, +either a fast-marching algorithm, with different neighborhoods for each. + +So far, the demo contains: +- Python: + - a purely functional pattern (functions which returns parametrized closures), + - the strategy pattern (composition of abstract interface), +- C++: + - purely functional (functions which returns parametrized lambdas), + - strategy (composition of abstract classes), + - policies (function parameters as templated variables), + - CRTP (Curiously Recurring Template Pattern) + +The algorithm machinery itself is located within the `code.*` files +and is not of major interest, +look for the other files for the architecture. diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt new file mode 100644 index 0000000..918eb53 --- /dev/null +++ b/cpp/CMakeLists.txt @@ -0,0 +1,35 @@ +# CMakeLists files in this project can +# refer to the root source directory of the project as ${EIKOS_SOURCE_DIR} and +# to the root binary directory of the project as ${EIKOS_BINARY_DIR}. +cmake_minimum_required (VERSION 2.8.11) +project (algopattern) + +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra -Wpedantic") + +# put binaries in the build directory +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) + +# Dump used compiler flags. +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +include_directories(.) + +enable_testing() + +set(repeat 10) +# set(repeat 2000) +file(WRITE ${CMAKE_BINARY_DIR}/test.sh "$1; for i in $(seq ${repeat}); do $1 > /dev/null; done\n") + + +add_executable(strategy strategy.cpp) +add_test(NAME strategy COMMAND sh ${CMAKE_BINARY_DIR}/test.sh $) + +add_executable(functional functional.cpp) +add_test(NAME functional COMMAND sh ${CMAKE_BINARY_DIR}/test.sh $) + +add_executable(policies policies.cpp) +add_test(NAME policies COMMAND sh ${CMAKE_BINARY_DIR}/test.sh $) + +add_executable(crtp crtp.cpp) +add_test(NAME crtp COMMAND sh ${CMAKE_BINARY_DIR}/test.sh $) + diff --git a/cpp/code.h b/cpp/code.h new file mode 100644 index 0000000..ec50f29 --- /dev/null +++ b/cpp/code.h @@ -0,0 +1,322 @@ +#include +#include +#include +#include + +//! x, y. +using point_t = std::pair; +//! Time to go to a given point. +using costs_t = std::map; +//! Concept to compare points. +using comp_t = std::function< bool( const point_t lhs, const point_t rhs) >; +//! The priority queue that permits an efficient single pass over the domain. +using front_t = std::priority_queue,comp_t>; +//! Set of points "around". +using neighbors_t = std::vector; + +point_t make_point(double x, double y) {return std::make_pair(x,y);} +double& x( point_t& p) {return p.first ;} +double& y( point_t& p) {return p.second;} +double x(const point_t& p) {return p.first ;} +double y(const point_t& p) {return p.second;} +//! Euclidean distance. +double distance(point_t u, point_t v) {return sqrt( (x(u)-x(v))*(x(u)-x(v)) + (y(u)-y(v))*(y(u)-y(v)) );} + +/** Test if a cost has already been computed for a given point. + + \param p The considered point. + \param costs The costs map in which to look for the given point. + */ +bool has_cost(point_t p, const costs_t & costs) +{ + // The map does not have the point as key, + // or it have an infinite cost (not use at the moment). + return costs.find(p) != costs.end() and costs.at(p) < std::numeric_limits::infinity(); +} + + +/** \defgroup Tour Tools to easily build a sequence of consecutive pairs of iterators across a given container. + @{ + */ + +/** State iterator of a tour. + + An iterator that maintain its next as a separate member and ends with next = begin. + */ +template +class tour_iterator +{ + public: + using ref = typename std::iterator_traits::reference; + using pair = std::pair; + + tour_iterator(FwdIt first, FwdIt last, bool end=false) + : _first(first), _cur(first), _next(first == last ? last : std::next(first)), _last(last), + _tour_ended(first == last ? true : end) + {} + + bool operator!=(const tour_iterator & other) const + { + return /*_cur != other._cur and*/ _next != other._next; + } + + tour_iterator & operator++() + { + _cur++; + + if(not _tour_ended) { + _next++; + if( _next == _last ) { // Back loop. + _next = _first; + _tour_ended = true; + } + } else { + _cur = _last; + _next = _last; + } + + return *this; + } + + pair operator*() const + { + return pair(*_cur, *_next); + } + + private: + const FwdIt _first; + FwdIt _cur; + FwdIt _next; + const FwdIt _last; + bool _tour_ended; +}; + +/** Range of a tour. */ +template +class tour +{ + public: + tour(const FwdIt first, const FwdIt last) : _first(first), _last(last) { } + + tour_iterator begin() const + { + return tour_iterator(_first, _last, false); + } + + tour_iterator end() const + { + return tour_iterator(_last, _last, true); + } + + private: + const FwdIt _first; + const FwdIt _last; +}; + +/** Make a tour from a container. */ +template +auto make_tour(C& c) -> tour +{ + return tour(begin(c), end(c)); +} +/** @} Tour */ + + +/** \defgroup Algorithm The actual code necessary for all Dijkstra/Fast Marching variants. + + The algorithms operates on an IMPLICIT square grid defined by a rectangle and an orthogonal step size. + They propagate a front of computed isotropic costs (stored in a [sparse] map), from a single seed. + + @{ +*/ + +/** Compute coordinates of the neighbors of the given point, according to grid parameters and directions. + + Directions are supposed to be given in a specific order + (in this project, the transit_in_simplex functions will necessitate clockwise order). + + \param p The considered point. + \param grid_step Length of an orthogonal edge of the grid. + \param pmin Corner point of the grid, with minimal coordinates. + \param pmax Corner point of the grid, with maximal coordinates. + \return A sequence of neighbors points. + */ +inline neighbors_t neighbors_grid(const point_t & p, double grid_step, const point_t & pmin, const point_t & pmax, std::vector directions) +{ + neighbors_t neighbors; + for( point_t d : directions) { + point_t n; + x(n) = x(p) + x(d) * grid_step; + y(n) = y(p) + y(d) * grid_step; + if( x(pmin) <= x(n) and x(n) <= x(pmax) and y(pmin) <= y(n) and y(n) <= y(pmax) ) { + neighbors.push_back( n ); + } + } + assert(neighbors.size() >= 2); + return neighbors; +} + +/** Find the transit of minimal cost among the given edges. + + Edges are given as the considered point and the sequence of neighbors points. */ +inline double transit_on_edge(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) +{ + double mincost = std::numeric_limits::infinity(); + for( auto n : neighbors ) { + // Do not compute transition toward points without a cost + // (i.e. Supposedly, only toward the front). + if( has_cost(n, costs) ) { + // Cost of the transition from/to p from/to n. + double c = costs.at(n) + distance(p, n); + if( c < mincost ) { + mincost = c; + } + } + } + // Should be near the front (and thus have found a transit). + assert(mincost != std::numeric_limits::infinity()); + return mincost; +} + +/** Find the transit in minimal cost within the given simplexes. + + That is, find the minimal distance between the given point + and one of the edges formed by the sequence of pairs of neighbors. + Neighbors should thus be given in clockwise order. + The minimal transit is searched across 1/eps distances, + regularly spaced on each edge. */ +inline double transit_in_simplex(const point_t & p, const neighbors_t & neighbors, const costs_t & costs, double eps) +{ + double mincost = std::numeric_limits::infinity(); + + // Special case: having a single point with a cost in the neighborhood. + // e.g. the seed. + // This would make an empty tour and thus needs a special case. + auto with_cost = [&costs] (const point_t& i) {return has_cost(i,costs);}; + if( std::count_if(begin(neighbors), end(neighbors), with_cost) == 1 ) { + // There is only one possible transition. + auto in = std::find_if( begin(neighbors), end(neighbors), with_cost); + mincost = costs.at(*in) + distance(p,*in); + + } else { + for( auto edge : make_tour(neighbors) ) { + point_t pj = edge.first; + point_t pk = edge.second; + + // Do not compute transition toward points without a cost + // (i.e. only toward the front). + if( has_cost(pj, costs) and has_cost(pk, costs) ) { + // Cost of the transition from/to p from/to edge e. + // This is the simplest way to minimize the transit, even if not the most efficient. + for( double z=0; z<=1; z+=eps) { + double xj = x(pj); double yj = y(pj); + double xk = x(pk); double yk = y(pk); + double zx = ( z*xj + (1-z)*xk ); + double zy = ( z*yj + (1-z)*yk ); + point_t n; x(n) = zx, y(n)=zy; + + // Linear interpolation of costs. + double c = z*costs.at(pj) + (1-z)*costs.at(pk) + distance(p, n); + if( c < mincost ) { + mincost = c; + } + } // for z + + // If the front is reached on a single point. + } else if(has_cost(pj, costs) and not has_cost(pk, costs)) { + double c = costs.at(pj) + distance(p,pj); + if( c < mincost ) { + mincost = c; + } + } else if(not has_cost(pj, costs) and has_cost(pk, costs)) { + double c = costs.at(pk) + distance(p,pk); + if( c < mincost ) { + mincost = c; + } + } + } // for edge + } + + // Should be near the front (and thus have found a transit). + assert(mincost < std::numeric_limits::infinity()); + return mincost; +} + +/** Propagate the front from the given seed, during the given number of iterations. + + Iteratively accept points of minimal costs (see the transit function) in a neighborhood (see the neighbors function). + + \param seed The point with NULL cost. + \param iterations The maximum number of iterations. If ommitted, the costs of all the points of the grid will be computed. + \return The costs map: => +*/ +inline costs_t algo_run( + point_t seed, + unsigned int iterations, + std::function< neighbors_t(const point_t&) > neighbors, + std::function< double(const point_t &, const neighbors_t &, const costs_t &) > transit + ) +{ + costs_t costs; + + // Make a priority queue of considered nodes. + auto compare = [&costs](const point_t& lhs, const point_t& rhs) { return costs[lhs] > costs[rhs]; }; + front_t front(compare); + + // Start the front from the seed + costs[seed] = 0; + front.push(seed); + + unsigned int i=0; + while(i++ < iterations and not front.empty()) { + std::cout << "\r" << i << "/" << iterations; + + // Accept the node with the min cost and update neighbors. + // Accept the considered node with the min cost. + point_t accepted = front.top(); front.pop(); + // Consider neighbors of the accepted node. + neighbors_t around = neighbors(accepted); + assert(around.size()>0); + for( auto n : around ) { + // If no cost has been computed (i.e. the node is "open"). + if( not has_cost(n, costs)) { + // Compute costs. + costs[n] = transit(n, neighbors(n), costs); + front.push(n); + } + } + } + return costs; +} + +/** @} Algorithm */ + + + +/** Pretty print a costs map. */ +void grid_print( const costs_t & grid, point_t pmin, point_t pmax, double step, + std::ostream& out = std::cout, std::string sep = " ", std::string end = "\n", + unsigned int width = 5, char fill = ' ', unsigned int prec = 3) +{ + out << std::setw(width) << std::setfill(fill) << std::setprecision(prec); + + out << " x:"; + for( double px=x(pmin); px<=x(pmax); px+=step) { + out << sep << std::setw(width) << std::setfill(fill) << px; + } + out << end; + out << " y" << end;; + for( double py=y(pmax); py>=y(pmin); py-=step) { + out << std::setw(width) << std::setfill(fill) << py << ":"; + for( double px=x(pmin); px<=x(pmax); px+=step) { + point_t p; x(p)=px; y(p)=py; + if( has_cost(p,grid) ) { + out << sep << std::setw(width) << std::setfill(fill) << std::setprecision(prec) << grid.at(p); + } else { + out << sep << std::setw(width) << std::setfill(fill) << "."; + } + } + out << end; + } + out << end; +} diff --git a/cpp/crtp.cpp b/cpp/crtp.cpp new file mode 100644 index 0000000..b9b0a68 --- /dev/null +++ b/cpp/crtp.cpp @@ -0,0 +1,151 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "code.h" + + + +namespace neighbors { + template + struct Neighborhood + { + public: + Neighborhood( const point_t & pmin_, const point_t & pmax_) : pmin(pmin_), pmax(pmax_) {} + neighbors_t operator()(const point_t & p) + { + return static_cast(this)->call(p); + } + protected: + const point_t & pmin, pmax; + }; + + class quad_grid : public Neighborhood + { + public: + quad_grid(double grid_step_, const point_t & pmin, const point_t & pmax) : Neighborhood(pmin,pmax), grid_step(grid_step_) {} + neighbors_t call(const point_t & p) + { + // neighborhood (list) of relative locations (vector) + // Should be given in [clockwise] order. + std::vector directions{{1,0},{0,-1},{-1,0},{0,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + protected: + double grid_step; + }; + + class octo_grid : public Neighborhood + { + public: + octo_grid(double grid_step_, const point_t & pmin, const point_t & pmax) : Neighborhood(pmin,pmax), grid_step(grid_step_) {} + neighbors_t call(const point_t & p) + { + std::vector directions{{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + protected: + double grid_step; + }; +} + +namespace transit { + template + class HopfLax + { + public: + double operator()(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return static_cast(this)->call(p,neighbors,costs); + } + }; + + class on_edge : public HopfLax + { + public: + double call(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_on_edge(p,neighbors,costs); + } + }; + + class in_simplex : public HopfLax + { + public: + in_simplex( double epsilon ) : eps(epsilon) {assert(0 < epsilon and epsilon < 1);} + double call(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_in_simplex(p,neighbors,costs,eps); + } + protected: + double eps; + }; +} + + +template +class algo +{ + protected: + N & neighbors; + T & transit; + public: + algo(N & neighbors_, T & hl) : neighbors(neighbors_), transit(hl) {} + costs_t operator()(point_t seed, unsigned int iterations) + { + return algo_run(seed, iterations, std::ref(this->neighbors), std::ref(this->transit)); + } +}; + + +int main() +{ + point_t seed; x(seed)= 0;y(seed)= 0; + point_t pmin; x(pmin)=-5;y(pmin)=-5; + point_t pmax; x(pmax)=15;y(pmax)=15; + assert(x(seed) <= x(pmax)); + assert(x(seed) >= x(pmin)); + assert(y(seed) <= y(pmax)); + assert(y(seed) >= y(pmin)); + double step = 1; + unsigned int maxit=300; + double eps = 1/100.0; + + neighbors::quad_grid four (step, pmin, pmax); + neighbors::octo_grid eight(step, pmin, pmax); + + transit::on_edge graph; + transit::in_simplex mesh(eps); + + algo dijkstra4(four,graph); + std::cout << "Dijkstra, 4 neighbors" << std::endl; + costs_t cd4 = dijkstra4(seed, maxit); + std::cout << std::endl; + grid_print(cd4, pmin, pmax, step); + + algo fast_marching4(four,mesh); + std::cout << "Fast marching, 4 neighbors" << std::endl; + costs_t cfm4 = fast_marching4(seed, maxit); + std::cout << std::endl; + grid_print(cfm4, pmin, pmax, step); + + algo dijkstra8(eight,graph); + std::cout << "Dijkstra, 8 neighbors" << std::endl; + costs_t cd8 = dijkstra8(seed, maxit); + std::cout << std::endl; + grid_print(cd8, pmin, pmax, step); + + algo fast_marching8(eight,mesh); + std::cout << "Fast marching, 8 neighbors" << std::endl; + costs_t cfm8 = fast_marching8(seed, maxit); + std::cout << std::endl; + grid_print(cfm8, pmin, pmax, step); +} diff --git a/cpp/functional.cpp b/cpp/functional.cpp new file mode 100644 index 0000000..3f4e5b2 --- /dev/null +++ b/cpp/functional.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "code.h" + +namespace neighbors { + + using Neighborhood = std::function< neighbors_t(const point_t&) >; + + template + Neighborhood make(F f, const point_t& pmin, const point_t& pmax, double grid_step, Fargs...args) + { + return [f,pmin,pmax,grid_step,args...] + (const point_t& p) + ->neighbors_t + { return f(p, pmin, pmax, grid_step, args...); }; + } + + neighbors_t quad_grid(const point_t& p, const point_t& pmin, const point_t& pmax, double grid_step) + { + // neighborhood (list) of relative locations (vector) + // Should be given in [clockwise] order. + std::vector directions{{1,0},{0,-1},{-1,0},{0,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + + neighbors_t octo_grid(const point_t& p, const point_t& pmin, const point_t& pmax, double grid_step) + { + std::vector directions{{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } +} + +namespace transit { + + using HopfLax = std::function< double(const point_t &, const neighbors_t , const costs_t &) >; + + template + HopfLax make(F f, Fargs...args) + { + return [f,args...] + (const point_t & p, const neighbors_t neighbors, const costs_t & costs) + ->double + { return f(p, neighbors, costs, args...); }; + } + + double on_edge(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_on_edge(p,neighbors,costs); + } + + double in_simplex(const point_t & p, const neighbors_t & neighbors, const costs_t & costs, double epsilon) + { + return transit_in_simplex(p,neighbors,costs,epsilon); + } + +} + +costs_t algo(neighbors::Neighborhood neighbors, transit::HopfLax transit, point_t seed, unsigned int iterations) +{ + return algo_run(seed, iterations, neighbors, transit); +} + + +int main() +{ + point_t seed; x(seed)= 0;y(seed)= 0; + point_t pmin; x(pmin)=-5;y(pmin)=-5; + point_t pmax; x(pmax)=15;y(pmax)=15; + assert(x(seed) <= x(pmax)); + assert(x(seed) >= x(pmin)); + assert(y(seed) <= y(pmax)); + assert(y(seed) >= y(pmin)); + double step = 1; + unsigned int maxit=300; + double eps = 1/100.0; + + auto four = neighbors::make(neighbors::quad_grid, pmin, pmax, step); + auto eight = neighbors::make(neighbors::octo_grid, pmin, pmax, step); + + auto graph = transit::make(transit::on_edge); + auto mesh = transit::make(transit::in_simplex, eps); + + std::cout << "Dijkstra, 4 neighbors" << std::endl; + costs_t cd4 = algo(four, graph, seed, maxit); + std::cout << std::endl; + grid_print(cd4, pmin, pmax, step); + + std::cout << "Fast marching, 4 neighbors" << std::endl; + costs_t cfm4 = algo(four, mesh, seed, maxit); + std::cout << std::endl; + grid_print(cfm4, pmin, pmax, step); + + std::cout << "Dijkstra, 8 neighbors" << std::endl; + costs_t cd8 = algo(eight, graph, seed, maxit); + std::cout << std::endl; + grid_print(cd8, pmin, pmax, step); + + std::cout << "Fast marching, 8 neighbors" << std::endl; + costs_t cfm8 = algo(eight, mesh, seed, maxit); + std::cout << std::endl; + grid_print(cfm8, pmin, pmax, step); +} diff --git a/cpp/policies.cpp b/cpp/policies.cpp new file mode 100644 index 0000000..e24fb7c --- /dev/null +++ b/cpp/policies.cpp @@ -0,0 +1,162 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "code.h" + +namespace neighbors { + + template + class quad_grid + { + protected: + const double grid_step = STEP; + const point_t pmin = {PMIN_X, PMIN_Y}; + const point_t pmax = {PMAX_X, PMAX_Y}; + + neighbors_t neighbors(const point_t & p) + { + // neighborhood (list) of relative locations (vector) + // Should be given in [clockwise] order. + std::vector directions{{1,0},{0,-1},{-1,0},{0,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + }; + + template + class octo_grid + { + protected: + const double grid_step = STEP; + const point_t pmin = {PMIN_X, PMIN_Y}; + const point_t pmax = {PMAX_X, PMAX_Y}; + + neighbors_t neighbors(const point_t & p) + { + std::vector directions{{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + }; +} + +namespace transit { + + class on_edge + { + protected: + double transit(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_on_edge(p,neighbors,costs); + } + }; + + template + class in_simplex + { + protected: + const double eps; + in_simplex() : eps(static_cast(EPS::num)/static_cast(EPS::den)) {} + + double transit(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_in_simplex(p,neighbors,costs, this->eps); + } + }; +} + + +template +class algo : private NeighborsPolicy, private TransitPolicy +{ + + public: + using NeighborsPolicy::neighbors; + using TransitPolicy::transit; + + costs_t operator()(point_t seed, unsigned int iterations) + { + costs_t costs; + + // Make a priority queue of considered nodes. + auto compare = [&costs](const point_t& lhs, const point_t& rhs) { return costs[lhs] > costs[rhs]; }; + front_t front(compare); + + // Start the front from the seed + costs[seed] = 0; + front.push(seed); + + unsigned int i=0; + while(i++ < iterations and not front.empty()) { + std::cout << "\r" << i << "/" << iterations; + + // Accept the node with the min cost and update neighbors. + // Accept the considered node with the min cost. + point_t accepted = front.top(); front.pop(); + // Consider neighbors of the accepted node. + neighbors_t around = neighbors(accepted); + assert(around.size()>0); + for( auto n : around ) { + // If no cost has been computed (i.e. the node is "open"). + if( not has_cost(n, costs)) { + // Compute costs. + costs[n] = transit(n, neighbors(n), costs); + front.push(n); + } + } + } + return costs; + } +}; + + +int main() +{ + point_t seed; x(seed)= 0;y(seed)= 0; + point_t pmin; x(pmin)=-5;y(pmin)=-5; + point_t pmax; x(pmax)=15;y(pmax)=15; + assert(x(seed) <= x(pmax)); + assert(x(seed) >= x(pmin)); + assert(y(seed) <= y(pmax)); + assert(y(seed) >= y(pmin)); + double step = 1; + unsigned int maxit=300; + + using four = neighbors::quad_grid; + using eight = neighbors::octo_grid<1, -5, -5, 15, 15>; + + using graph = transit::on_edge; + using mesh = transit::in_simplex>; + + algo dijkstra4; + std::cout << "Dijkstra, 4 neighbors" << std::endl; + costs_t cd4 = dijkstra4(seed, maxit); + std::cout << std::endl; + grid_print(cd4, pmin, pmax, step); + + algo fast_marching4; + std::cout << "Fast marching, 4 neighbors" << std::endl; + costs_t cfm4 = fast_marching4(seed, maxit); + std::cout << std::endl; + grid_print(cfm4, pmin, pmax, step); + + algo dijkstra8; + std::cout << "Dijkstra, 8 neighbors" << std::endl; + costs_t cd8 = dijkstra8(seed, maxit); + std::cout << std::endl; + grid_print(cd8, pmin, pmax, step); + + algo fast_marching8; + std::cout << "Fast marching, 8 neighbors" << std::endl; + costs_t cfm8 = fast_marching8(seed, maxit); + std::cout << std::endl; + grid_print(cfm8, pmin, pmax, step); +} diff --git a/cpp/strategy.cpp b/cpp/strategy.cpp new file mode 100644 index 0000000..3f8a2a2 --- /dev/null +++ b/cpp/strategy.cpp @@ -0,0 +1,160 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "code.h" + +namespace neighbors { + /** A neighborhood returns a sequence of points "around" a given point. */ + class Neighborhood + { + public: + Neighborhood( const point_t & pmin_, const point_t & pmax_) : pmin(pmin_), pmax(pmax_) {} + neighbors_t operator()(const point_t & p) {return call(p);} + protected: + const point_t & pmin, pmax; + virtual neighbors_t call(const point_t & p) = 0; + }; + + /** The four closest orthogonal neighbors on a square grid. */ + class quad_grid : public Neighborhood + { + public: + quad_grid(double grid_step_, const point_t & pmin, const point_t & pmax) : Neighborhood(pmin,pmax), grid_step(grid_step_) {} + protected: + double grid_step; + virtual neighbors_t call(const point_t & p) + { + // neighborhood (list) of relative locations (vector) + // Should be given in [clockwise] order. + std::vector directions{{1,0},{0,-1},{-1,0},{0,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + }; + + /** The eights closests neighbors on a square grid. + + The four orthogonal neighbors + the four diagonal ones. */ + class octo_grid : public Neighborhood + { + public: + octo_grid(double grid_step_, const point_t & pmin, const point_t & pmax) : Neighborhood(pmin,pmax), grid_step(grid_step_) {} + protected: + double grid_step; + virtual neighbors_t call(const point_t & p) + { + std::vector directions{{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1}}; + return neighbors_grid(p,grid_step,pmin,pmax,directions); + } + }; +} + +namespace transit { + /** The Hopf-Lax operator computes the minimal transition from/to a given point to/from its boundary. + + The boundary of the point is discretized and given a a sequence of points. */ + class HopfLax + { + public: + double operator()(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) {return call(p,neighbors,costs);} + protected: + virtual double call(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) = 0; + }; + + /** Search the minimal transition on a boundary discretized as points. */ + class on_edge : public HopfLax + { + protected: + virtual double call(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_on_edge(p,neighbors,costs); + } + }; + + /** Search the minimal transition on a boundary discretized as segments. */ + class in_simplex : public HopfLax + { + public: + in_simplex( double epsilon ) : eps(epsilon) {assert(0 < epsilon and epsilon < 1);} + protected: + double eps; + virtual double call(const point_t & p, const neighbors_t & neighbors, const costs_t & costs) + { + return transit_in_simplex(p,neighbors,costs,eps); + } + }; +} + +/** An algorithm is a combination of a neighborhood and an Hopf-Lax operator. */ +class algo +{ + protected: + neighbors::Neighborhood & neighbors; + transit::HopfLax & transit; + public: + algo(neighbors::Neighborhood & neighbors_, transit::HopfLax & hl) : neighbors(neighbors_), transit(hl) {} + virtual costs_t operator()(point_t seed, unsigned int iterations) + { + return algo_run(seed, iterations, std::ref(this->neighbors), std::ref(this->transit)); + } +}; + + +int main() +{ + point_t seed; x(seed)= 0;y(seed)= 0; + point_t pmin; x(pmin)=-5;y(pmin)=-5; + point_t pmax; x(pmax)=15;y(pmax)=15; + assert(x(seed) <= x(pmax)); + assert(x(seed) >= x(pmin)); + assert(y(seed) <= y(pmax)); + assert(y(seed) >= y(pmin)); + double step = 1; + unsigned int maxit=300; + double eps = 1/100.0; + + // Instanciate 2 different functors for each function of the algorithm. + neighbors::quad_grid four (step, pmin, pmax); + neighbors::octo_grid eight(step, pmin, pmax); + + transit::on_edge graph; + transit::in_simplex mesh(eps); + + // Instanciate the 2*2 possible algo. + + // Instanciate an algo propagating the front across a 4-neighborhood, on edges. + // That is, a Dijkstra's algorithm. + algo dijkstra4(four,graph); + + // Run and print. + std::cout << "Dijkstra, 4 neighbors" << std::endl; + costs_t cd4 = dijkstra4(seed, maxit); + std::cout << std::endl; + grid_print(cd4, pmin, pmax, step); + + algo fast_marching4(four,mesh); + std::cout << "Fast marching, 4 neighbors" << std::endl; + costs_t cfm4 = fast_marching4(seed, maxit); + std::cout << std::endl; + grid_print(cfm4, pmin, pmax, step); + + algo dijkstra8(eight,graph); + std::cout << "Dijkstra, 8 neighbors" << std::endl; + costs_t cd8 = dijkstra8(seed, maxit); + std::cout << std::endl; + grid_print(cd8, pmin, pmax, step); + + algo fast_marching8(eight,mesh); + std::cout << "Fast marching, 8 neighbors" << std::endl; + costs_t cfm8 = fast_marching8(seed, maxit); + std::cout << std::endl; + grid_print(cfm8, pmin, pmax, step); +} diff --git a/python/code.py b/python/code.py new file mode 100644 index 0000000..be89452 --- /dev/null +++ b/python/code.py @@ -0,0 +1,206 @@ +import sys +import math +from queue import PriorityQueue + + +def x(p): + return p[0] + + +def y(p): + return p[1] + + +def distance(u,v): + return math.sqrt( (x(u)-x(v))*(x(u)-x(v)) + (y(u)-y(v))*(y(u)-y(v)) ) + + +def tour(lst): + """Yield a sequence of consecutive pairs of items across a given container.""" + assert(len(lst)>=2) + # consecutive pairs in lst + last-to-first element + for a,b in zip(lst, lst[1:] + [lst[0]]): + yield (a,b) + + +# This is a very bad way to implement frange, +# you should use nympy.linspace in real code… +def frange(x, y, s): + while x < y: + yield x + x += s + + +def neighbors_grid(p, grid_step, pmin, pmax, directions): + """ + Compute coordinates of the neighbors of the given point, according to grid parameters and directions. + + Directions are supposed to be given in a specific order + (in this project, the transit_in_simplex functions will necessitate clockwise order). + + Args: + point: The considered point. + grid_step: Length of an orthogonal edge of the grid. + pmin: Corner point of the grid, with minimal coordinates. + pmax: Corner point of the grid, with maximal coordinates. + + Returns: + A sequence of neighbors points. + """ + neighbors = [] + for d in directions: + nx = x(p)+x(d)*grid_step + ny = y(p)+y(d)*grid_step + n = (nx, ny) + if x(pmin) <= x(n) <= x(pmax) and y(pmin) <= y(n) <= y(pmax): + neighbors.append(n) + assert(len(neighbors) >= 2) + return neighbors + + +def transit_on_edge(point, neighbors, costs): + """ + Find the transit of minimal cost among the given edges. + + Edges are given as the considered point and the sequence of neighbors points. + """ + mincost = float("inf") + for n in neighbors: + # Do not compute transition toward points without a cost + # (i.e. Supposedly, only toward the front). + if n in costs: + # Cost of the transition from/to p from/to n. + c = costs[n] + distance(point,n) + if c < mincost: + mincost = c + # Should be near the front (and thus have found a transit). + assert(mincost != float("inf")) + return mincost + + +def transit_in_simplex(p, neighbors, costs, eps): + mincost = float("inf") + + # Special case: having a single point with a cost in the neighborhood. + # e.g. the seed. + # This would make an empty tour and thus needs a special case. + with_costs = [n for n in neighbors if n in costs] + if len(with_costs) == 1: + # There is only one possible transition. + i = with_costs[0] + mincost = costs[i] + distance(p,i) + + else: + for edge in tour(neighbors): + pj, pk = edge + + # Do not compute transition toward points without a cost + # (i.e. only toward the front). + if pj in costs and pk in costs: + # Cost of the transition from/to p from/to edge e. + # This is the simplest way to minimize the transit, even if not the most efficient. + for z in frange(0,1,eps): + xj,yj = pj + xk,yk = pk + zx = z*xj + (1-z)*xk + zy = z*yj + (1-z)*yk + n = (zx,zy) + + # Linear interpolation of costs. + c = z*costs[pj] + (1-z)*costs[pk] + distance(p,n) + if c < mincost: + mincost = c + + # If the front is reached on a single point. + elif pj in costs and pk not in costs: + c = costs[pj] + distance(p,pj) + if c < mincost: + mincost = c + + elif pj not in costs and pk in costs: + c = costs[pk] + distance(p,pk) + if c < mincost: + mincost = c + + # Should be near the front (and thus have found a transit). + assert(mincost != float("inf")) + return mincost + + +def algo_run(seed, iterations, neighbors, transit_func): + """ + Propagate the front from the given seed, during the given number of iterations. + + Iteratively accept points of minimal costs (see the transit function) in a neighborhood (see the neighbors function). + + Args: + seed: The point with NULL cost. + iterations: The maximum number of iterations. If ommitted, the costs of all the points of the grid will be computed. + + Returns: + The costs map: => + """ + costs = {} + + # Make a priority queue of considered nodes. + class Prioritize(PriorityQueue): + def __init__(self, keyf): + super().__init__() + self.keyf = keyf + + def push(self, i): + # Put (priority,item) + super().put( (self.keyf(i), i) ) + + def pop(self): + # Return and remove + return super().get()[1] + + on_cost = lambda n: costs[n] + front = Prioritize(on_cost) + + # Start the front from the seed + costs[seed] = 0 + front.push(seed) + i=0 + while i < iterations and not front.empty(): + print("{} iterations".format(i), end='\r') + # Accept the node with the min cost and update neighbors. + # Accept the considered node with the min cost. + accepted = front.pop() + # Consider neighbors of the accepted node. + around = neighbors(accepted) + assert(len(around)>0) + for n in around: + # If no cost has been computed (i.e. the node is "open"). + if n not in costs: + # Compute costs. + costs[n] = transit_func(n, neighbors(n), costs) + front.push(n) + + i += 1 + + return costs + + +def grid_print(grid, pmin, pmax, step, + out = sys.stdout, sep = " ", end = "\n", + width = 5, fill = ' ', prec = 3): + """Pretty print a cost map""" + + print(" x:", end="", file=out) + for px in range(x(pmin),x(pmax),step): + print("{sep}{:{fill}>{width}}".format(px,sep=sep,fill=fill,width=width,prec=prec), end="", file=out) + + print(end=end) + print(" y",end=end) + for py in range(y(pmax),y(pmin),-step): + print("{:{fill}>{width}}:".format(py,fill=fill,width=width,prec=prec), end="", file=out) + for px in range(x(pmin),x(pmax),step): + p=(px,py) + if p in grid: + print("{sep}{:{fill}>{width}.{prec}}".format(float(grid[p]),sep=sep,fill=fill,width=width,prec=prec), end="", file=out) + else: + print("{sep}{:{fill}>{width}}".format(".",sep=sep,fill=fill,width=width,prec=prec), end="", file=out) + print(end=end) + print(end=end) diff --git a/python/functional.py b/python/functional.py new file mode 100644 index 0000000..c72ea67 --- /dev/null +++ b/python/functional.py @@ -0,0 +1,64 @@ +import code + +def neighbors(op, pmin, pmax, grid_step, **kwargs): + def f(p): + return op(p, pmin, pmax, grid_step, **kwargs) + return f + +def quad_grid(p, pmin, pmax, grid_step): + directions = ((1,0),(0,-1),(-1,0),(0,1)) + return code.neighbors_grid(p, grid_step, pmin, pmax, directions) + +def octo_grid(p, pmin, pmax, grid_step): + directions = ((1,0),(1,-1),(0,-1),(-1,-1),(-1,0),(-1,1),(0,1),(1,1)) + return code.neighbors_grid(p, grid_step, pmin, pmax, directions) + + +def transit(op, **kwargs): + def f(p, neighbors, costs): + return op(p, neighbors, costs, **kwargs) + return f + +def on_edge(p, neighbors, costs): + return code.transit_on_edge(p, neighbors, costs) + +def in_simplex(p, neighbors, costs, epsilon): + return code.transit_in_simplex(p, neighbors, costs, epsilon) + + +def algo(neighbors, transit, seed, iterations): + return code.algo_run( seed, iterations, neighbors, transit) + + +if __name__ == "__main__": + + seed = (0,0) + pmin = (-5,-5) + pmax = (15,15) + step = 1 + maxit = 300 + eps = 1/100 + + four = neighbors(quad_grid, pmin, pmax, step) + eight = neighbors(octo_grid, pmin, pmax, step) + + graph = transit(on_edge) + mesh = transit(in_simplex, epsilon=eps) + + print("Dijkstra, 4 neighbors") + cd4 = algo(four, graph, seed, maxit) + code.grid_print(cd4, pmin, pmax, step) + + print("Fast marching, 4 neighbors") + cfm4 = algo(four, mesh, seed, maxit) + code.grid_print(cfm4, pmin, pmax, step) + + print("Dijkstra, 8 neighbors") + cd8 = algo(eight, graph, seed, maxit) + code.grid_print(cd8, pmin, pmax, step) + + print("Fast marching, 8 neighbors") + cfm8 = algo(eight, mesh, seed, maxit) + code.grid_print(cfm8, pmin, pmax, step) + + diff --git a/python/strategy.py b/python/strategy.py new file mode 100644 index 0000000..42c0287 --- /dev/null +++ b/python/strategy.py @@ -0,0 +1,101 @@ +import abc +import code + + +class neighbors: + class Neighborhood(object, metaclass=abc.ABCMeta): + def __init__(self, pmin, pmax): + self.pmin = pmin + self.pmax = pmax + + @abc.abstractmethod + def __call__(self, p): + raise NotImplementedError + + + class quad_grid(Neighborhood): + def __init__(self, grid_step, pmin, pmax): + super().__init__(pmin, pmax) + self.grid_step = grid_step + + def __call__(self, p): + directions = ((1,0),(0,-1),(-1,0),(0,1)) + return code.neighbors_grid(p, self.grid_step, pmin, pmax, directions) + + + class octo_grid(Neighborhood): + def __init__(self, grid_step, pmin, pmax): + super().__init__(pmin, pmax) + self.grid_step = grid_step + + def __call__(self, p): + directions = ((1,0),(1,-1),(0,-1),(-1,-1),(-1,0),(-1,1),(0,1),(1,1)) + return code.neighbors_grid(p, self.grid_step, pmin, pmax, directions) + + +class transit: + class HopfLax(object, metaclass=abc.ABCMeta): + + @abc.abstractmethod + def __call__(self, p, neighbors, costs): + raise NotImplementedError + + + class on_edge(HopfLax): + def __call__(self, p, neighbors, costs): + return code.transit_on_edge(p, neighbors, costs) + + + class in_simplex(HopfLax): + def __init__(self, epsilon): + self.eps = epsilon + + def __call__(self, p, neighbors, costs): + return code.transit_in_simplex(p, neighbors, costs, self.eps) + + +class algo(object): + def __init__(self, neighbors, transit): + self.neighbors = neighbors + self.transit = transit + + def __call__(self, seed, iterations): + return code.algo_run( seed, iterations, self.neighbors, self.transit) + + +if __name__ == "__main__": + + seed = (0,0) + pmin = (-5,-5) + pmax = (15,15) + step = 1 + maxit = 300 + eps = 1/100 + + four = neighbors.quad_grid(step, pmin, pmax) + eight = neighbors.octo_grid(step, pmin, pmax) + + graph = transit.on_edge() + mesh = transit.in_simplex(epsilon=eps) + + dijkstra4 = algo(four, graph) + print("Dijkstra, 4 neighbors") + cd4 = dijkstra4(seed, maxit) + code.grid_print(cd4, pmin, pmax, step) + + fast_marching4 = algo(four, mesh) + print("Fast marching, 4 neighbors") + cfm4 = fast_marching4(seed, maxit) + code.grid_print(cfm4, pmin, pmax, step) + + dijkstra8 = algo(eight, graph) + print("Dijkstra, 8 neighbors") + cd8 = dijkstra8(seed, maxit) + code.grid_print(cd8, pmin, pmax, step) + + fast_marching8 = algo(eight, mesh) + print("Fast marching, 8 neighbors") + cfm8 = fast_marching8(seed, maxit) + code.grid_print(cfm8, pmin, pmax, step) + +