Distance between angles as points This code calculates the distance between angles, particularly for n-tuples of angles. One example where this situation occurs is as follows:

I'm using a 2 arms, one 6 degree of freedom and the other 7 dof with revolute (rotating) joints. The position of the arm can be accurately represented as an n-d point representing the angle of each joint, with no need for orientation/pose on the surface of a torus, with 1 dimension for each joint. 

Here is a simple image explaining the 2D case.

Therefore I don't need to worry about orientation but probably just the envelope/box which I believe is used for the data structure and the points themselves.

Here is an implementation, which should be easy to template for more general situations:

typedef boost::array&lt;double,6&gt; ArmPos;

template&lt;typename T&gt;
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&lt;T&gt;(),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&lt;T&gt;())) - 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&lt;typename T&gt;
struct square
{
    T operator()(const T&amp; Left, const T&amp; Right) const
    {   
        return (Left + Right*Right);
    }
};

namespace boost { namespace geometry {


double comparable_distance(ArmPos const&amp; p1, ArmPos const&amp; p2 ) {
    ArmPos diff;
    boost::transform(p1,p2,diff.begin(),std::minus&lt;ArmPos::value_type&gt;());
    boost::transform(diff,diff.begin(),&amp;normalizeRadiansPiToMinusPi&lt;ArmPos::value_type&gt;);
    return boost::accumulate(diff,0,square&lt;ArmPos::value_type&gt;());
}



template&lt;typename Box&gt;
double comparable_distance(ArmPos const&amp; armpos, Box const&amp; box ){
    namespace bg = boost::geometry;
    ArmPos normAP = normalizeRadiansPiToMinusPi(armpos);
    ArmPos mindiff;
    boost::transform(normAP,bg::get&lt;bg::min_corner&gt;(box),mindiff.begin(),std::minus&lt;ArmPos::value_type&gt;());
    boost::transform(mindiff,mindiff.begin(),&amp;normalizeRadiansPiToMinusPi&lt;ArmPos::value_type&gt;);
    ArmPos maxdiff;
    boost::transform(normAP,bg::get&lt;bg::max_corner&gt;(box),maxdiff.begin(),std::minus&lt;ArmPos::value_type&gt;());
    boost::transform(maxdiff,maxdiff.begin(),&amp;normalizeRadiansPiToMinusPi&lt;ArmPos::value_type&gt;);

    ArmPos::value_type final_distance = 0.0;
    for(int i = 0; i &lt; armpos.size(); ++i){
        if(mindiff[i] &gt;= 0.0 &amp;&amp; maxdiff[i] &lt;= 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&lt;D&gt; - p&lt;D&gt;), (p&lt;D&gt; - max&lt;D&gt;)
}


Full code can be found in this gist.
This code is very hard to read. These are very simple geometric operations that you are programming (just things like subtracting vectors, or finding a vector's squared length) but their meaning is buried under a mountain of boilerplate.

Where you have:

boost::transform(p1,p2,diff.begin(),std::minus&lt;ArmPos::value_type&gt;());


could you find some way to write:

diff = p1 - p2;


instead? Similarly, where you have:

return boost::accumulate(diff,0,square&lt;ArmPos::value_type&gt;());


could you find some way to write:

return diff.lengthSquared();


instead? C++ supports operator overloading and virtual functions, so surely something along these lines would be possible.