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());
 }
Esempio n. 2
0
 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);
 }