instantiating boost::geometry::rtree with 6D torus points

compiler_output.bash

Scanning dependencies of target assignment3_context
[ 73%] Building CXX object cs436/assignment3/CMakeFiles/assignment3_context.dir/src/assignment3_context.cpp.o
In file included from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/geometry.hpp:21:0,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry.hpp:17,
                 from /home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:14,
                 from /home/ahundt/catkin_ws/src/cs436/assignment3/src/assignment3_context.cpp:1:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp: In instantiation of ‘boost::geometry::traits::coordinate_system<boost::array<double, 6ul>, void>’:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:71:21:   instantiated from ‘boost::geometry::core_dispatch::coordinate_system<boost::geometry::point_tag, boost::array<double, 6ul> >’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:93:17:   instantiated from ‘boost::geometry::coordinate_system<boost::array<double, 6ul> >’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:174:5:   instantiated from ‘boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >’
/home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:159:20:   instantiated from here
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:42:5: error: no matching function for call to ‘assertion_failed(mpl_::failed************ (boost::geometry::traits::coordinate_system<boost::array<double, 6ul>, void>::NOT_IMPLEMENTED_FOR_THIS_POINT_TYPE::************)(mpl_::assert_::types<boost::array<double, 6ul>, mpl_::na, mpl_::na, mpl_::na>))’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:42:5: note: candidate is:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/mpl/assert.hpp:83:5: note: template<bool C> int mpl_::assertion_failed(typename mpl_::assert<C>::type)
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp: In instantiation of ‘boost::geometry::core_dispatch::coordinate_system<boost::geometry::point_tag, boost::array<double, 6ul> >’:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:93:17:   instantiated from ‘boost::geometry::coordinate_system<boost::array<double, 6ul> >’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:174:5:   instantiated from ‘boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >’
/home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:159:20:   instantiated from here
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/core/coordinate_system.hpp:71:21: error: no type named ‘type’ in ‘struct boost::geometry::traits::coordinate_system<boost::array<double, 6ul>, void>’
In file included from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/closest_feature/range_to_range.hpp:23:0,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/distance/range_to_geometry_rtree.hpp:26,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/distance/multipoint_to_geometry.hpp:26,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/distance/implementation.hpp:25,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/comparable_distance/implementation.hpp:22,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/comparable_distance.hpp:23,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/buffer/turn_in_piece_visitor.hpp:24,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/buffer/buffered_piece_collection.hpp:32,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/detail/buffer/buffer_inserter.hpp:27,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/algorithms/buffer.hpp:32,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/geometry.hpp:48,
                 from /home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry.hpp:17,
                 from /home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:14,
                 from /home/ahundt/catkin_ws/src/cs436/assignment3/src/assignment3_context.cpp:1:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp: In constructor ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::members_holder::members_holder(const IndGet&, const ValEq&, const Parameters&) [with IndGet = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, ValEq = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >, Value = std::pair<boost::array<double, 6ul>, long unsigned int>, Parameters = boost::geometry::index::rstar<16ul, 4ul>, IndexableGetter = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, EqualTo = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >, Allocator = std::allocator<std::pair<boost::array<double, 6ul>, long unsigned int> >]’:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:232:46:   instantiated from ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::rtree(const parameters_type&, const indexable_getter&, const value_equal&) [with Value = std::pair<boost::array<double, 6ul>, long unsigned int>, Parameters = boost::geometry::index::rstar<16ul, 4ul>, IndexableGetter = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, EqualTo = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >, Allocator = std::allocator<std::pair<boost::array<double, 6ul>, long unsigned int> >, boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::parameters_type = boost::geometry::index::rstar<16ul, 4ul>, boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::indexable_getter = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::value_equal = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >]’
/home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:159:20:   instantiated from here
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1528:21: error: using invalid field ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::members_holder::values_count’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1528:21: error: using invalid field ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::members_holder::leafs_level’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1528:21: error: using invalid field ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::members_holder::root’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp: In member function ‘void boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::raw_destroy(boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>&) [with Value = std::pair<boost::array<double, 6ul>, long unsigned int>, Parameters = boost::geometry::index::rstar<16ul, 4ul>, IndexableGetter = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, EqualTo = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >, Allocator = std::allocator<std::pair<boost::array<double, 6ul>, long unsigned int> >, boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator> = boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >]’:
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:326:9:   instantiated from ‘boost::geometry::index::rtree<Value, Options, IndexableGetter, EqualTo, Allocator>::~rtree() [with Value = std::pair<boost::array<double, 6ul>, long unsigned int>, Parameters = boost::geometry::index::rstar<16ul, 4ul>, IndexableGetter = boost::geometry::index::indexable<std::pair<boost::array<double, 6ul>, long unsigned int> >, EqualTo = boost::geometry::index::equal_to<std::pair<boost::array<double, 6ul>, long unsigned int> >, Allocator = std::allocator<std::pair<boost::array<double, 6ul>, long unsigned int> >]’
/home/ahundt/catkin_ws/src/cs436/assignment3/src/rtree_graph_planner.hpp:159:20:   instantiated from here
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1321:9: error: ‘struct boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >::members_holder’ has no member named ‘root’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1325:13: error: ‘struct boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >::members_holder’ has no member named ‘root’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1327:13: error: ‘struct boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >::members_holder’ has no member named ‘root’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1329:9: error: ‘struct boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >::members_holder’ has no member named ‘values_count’
/home/ahundt/catkin_ws/build/cs436/assignment3/boost-prefix/include/boost/geometry/index/rtree.hpp:1330:9: error: ‘struct boost::geometry::index::rtree<std::pair<boost::array<double, 6ul>, long unsigned int>, boost::geometry::index::rstar<16ul, 4ul> >::members_holder’ has no member named ‘leafs_level’
make[2]: *** [cs436/assignment3/CMakeFiles/assignment3_context.dir/src/assignment3_context.cpp.o] Error 1
make[1]: *** [cs436/assignment3/CMakeFiles/assignment3_context.dir/all] Error 2
make: *** [all] Error 2
Invoking "make" failed

rtree_graph_planner.hpp

#ifndef RTREE_PLANNER_HPP_
#define RTREE_PLANNER_HPP_

#include <boost/random/mersenne_twister.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/fill.hpp>


#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/math/special_functions/fpclassify.hpp>


#include <boost/geometry/index/rtree.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

// Define properties for vertex
template <typename Point>
struct bg_vertex_property
{
    bg_vertex_property()
    {
        boost::geometry::assign_zero(location);
    }
    bg_vertex_property(Point const& loc)
        : location(loc)
    {
    }

    Point location;
};

// Define properties for edge
template <typename Line>
struct bg_edge_property
{
    bg_edge_property(Line const& line)
        : length(boost::geometry::length(line))
        , m_line(line)
    {
    }

    inline Line const& line() const
    {
        return m_line;
    }

    double length;
private :
    Line m_line;
};

//namespace pointindex {
    
    //typedef bg::model::point<double, 3, bg::cs::cartesian> point;
    typedef boost::array<double,6> ArmPos;
    typedef ArmPos point_type;
    typedef bg::model::box<point_type> box;
    typedef bg::model::referring_segment<point_type> line_type;
    //typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
    
    typedef boost::adjacency_list
            <
                boost::vecS, boost::vecS, boost::undirectedS
                , bg_vertex_property<point_type> // bundled
                , bg_edge_property<line_type>
            > graph_type;

    typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_type;

    typedef std::pair<point_type, vertex_type> rtree_value;
    typedef boost::geometry::index::rtree<rtree_value,bgi::rstar<16, 4> > knn_rtree_type;
    
//}




template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
  // copy the sign of the value in radians to the value of pi
  T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
  // set the value of rad to the appropriate signed value between pi and -pi
  rad = std::fmod(rad+signedPI,(boost::math::constants::two_pi<T>())) - signedPI;

  return rad;
} 


// functor for getting sum of previous result and square of current element
// source: http://stackoverflow.com/questions/1326118/sum-of-square-of-each-elements-in-the-vector-using-for-each
template<typename T>
struct square
{
    T operator()(const T& Left, const T& Right) const
    {   
        return (Left + Right*Right);
    }
};

namespace boost { namespace geometry {
    
    
double comparable_distance(ArmPos const& p1, ArmPos const& p2 ) {
    ArmPos diff;
    boost::transform(p1,p2,diff.begin(),std::minus<ArmPos::value_type>());
    boost::transform(diff,diff.begin(),&normalizeRadiansPiToMinusPi<ArmPos::value_type>);
    return boost::accumulate(diff,0,square<ArmPos::value_type>());
}



template<typename Box>
double comparable_distance(ArmPos const& armpos, Box const& box ){
    namespace bg = boost::geometry;
    ArmPos normAP = normalizeRadiansPiToMinusPi(armpos);
    ArmPos mindiff;
    boost::transform(normAP,bg::get<bg::min_corner>(box),mindiff.begin(),std::minus<ArmPos::value_type>());
    boost::transform(mindiff,mindiff.begin(),&normalizeRadiansPiToMinusPi<ArmPos::value_type>);
    ArmPos maxdiff;
    boost::transform(normAP,bg::get<bg::max_corner>(box),maxdiff.begin(),std::minus<ArmPos::value_type>());
    boost::transform(maxdiff,maxdiff.begin(),&normalizeRadiansPiToMinusPi<ArmPos::value_type>);
    
    ArmPos::value_type final_distance = 0.0;
    for(int i = 0; i < armpos.size(); ++i){
        if(mindiff[i] >= 0.0 && maxdiff[i] <= 0.0) continue; // between the min and max means "in the box" for this dimension
        ArmPos::value_type min_dist = std::min(std::abs(mindiff[i]),std::abs(maxdiff[i]));
        final_distance+=min_dist*min_dist;
    }
    
    return final_distance;
//    diff (min<D> - p<D>), (p<D> - max<D>)
}

}} // namespace boost::geometry

/// just a test
void instantiate_rtree_and_graph(){
  
    // comment these two lines to fix compiler error
    graph_type graph;
    knn_rtree_type rtree;
    
    // todo:
    // create vector of point_type
    // insert into graph
    // insert std::pair<point_type,vertex_type> into rtree
    // search for pairs of points that connect a start point to goal point without obstacles
}

#endif