// Authors: Unknown. Please, if you are the author of this file, or if you // know who are the authors of this file, let us know, so we can give the // adequate credits and/or get the adequate authorizations. #ifdef RODRIGUES_H namespace libNumerics { template matrix skew(const vector& v) { assert(v.nrow() == 3); matrix M(3,3); M(0,0) = M(1,1) = M(2,2) = 0; M(1,2) = -(M(2,1)=v(0)); M(2,0) = -(M(0,2)=v(1)); M(0,1) = -(M(1,0)=v(2)); return M; } template matrix rotation(vector w) { assert(w.nrow() == 3); T n = sqrt(w.qnorm()); T c = cos(n); matrix R = c*matrix::eye(3); if(n) { w /= n; R += skew(sin(n)*w); R += (1-c)*w*w.t(); } return R; } template vector rotationAxis(const matrix& R) { assert(R.nrow() == 3 && R.ncol() == 3); vector w(3); T n = acos(0.5*(R.tr()-1)); if(n == 0) w = 0; else { w(0) = R(2,1)-R(1,2); w(1) = R(0,2)-R(2,0); w(2) = R(1,0)-R(0,1); w *= n/(2*sin(n)); } return w; } } // libNumerics #endif // RODRIGUES_H