22 real m,q,p,sp,theta,ctheta,stheta,l1,l2,l3,a2,a1,a0,dem,b2,b1,b0;
34 A = (R.transpose())*R;
37 Q = A-m*Matrix3::Identity(3,3);
38 q = Q.determinant()/2;
39 p=(Q.array()*Q.array()).sum()/6;
49 theta = atan2(sqrt(abs(p*p*p-q*q)),q)/3;
60 l1 = abs(m + 2*sp*ctheta);
61 l2 = abs(m - sp*(ctheta+sqrt(3)*stheta));
62 l3 = abs(m - sp*(ctheta-sqrt(3)*stheta));
71 a1 = sqrt(l1*l2)+sqrt(l1*l3)+sqrt(l3*l2);
72 a2 = sqrt(l1)+sqrt(l2)+sqrt(l3);
84 b0 = (a2*a1*a1 -a0*(a2*a2 +a1))/dem;
85 b1 = (a0+a2*(a2*a2-2*a1))/dem;
94 U = b2*A*A-b1*A+b0*Matrix3::Identity(3,3);
Follow us!