# 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<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.

## 1 Answer

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.