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());
}
Transform3D Transform3D::createRotationZ(double radians) {
Transform3D transform;
transform.submat(0,0,2,2) = Rotation3D::createRotationZ(radians);
return transform;
}