Distance between angles as points

Question

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<double,6> ArmPos;
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>)
}

Full code can be found in this gist.

Answer 1

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<ArmPos::value_type>());

could you find some way to write:

diff = p1 - p2;

instead? Similarly, where you have:

return boost::accumulate(diff,0,square<ArmPos::value_type>());

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.