float Transform3D::norm(Transform3D T){
float pos_norm = arma::norm(T.translation());
UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
float angle = q.getAngle();
//TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
float Rotation3D::norm(Rotation3D T) {
UnitQuaternion q = UnitQuaternion(T);
// Get angle between -2Pi and 2pi
float angle = q.getAngle();
// Just want magnitude
float theta = std::fabs(angle);
// But rotating more that Pi in one direction is equivalent to a rotation in the other direction
return std::fmin(2 * M_PI - theta, theta);
}