ExactMethod.cpp
Go to the documentation of this file.
1 #include "methods.h"
2 
3 #include <cmath>
4 
13 {
14  /* std includes definitions of these functions both for floats and doubles */
15  using std::sqrt;
16  using std::abs;
17  using std::cos;
18  using std::sin;
19  using std::atan2;
20 
21  Matrix3 A,Q,U;
22  real m,q,p,sp,theta,ctheta,stheta,l1,l2,l3,a2,a1,a0,dem,b2,b1,b0;
23 
24  /* Equivalent Matlab code:
25 
26  A = R'*R;
27 
28  m = trace(A)/3;
29  Q = A-m*eye(3);
30  q = det(Q)/2;
31  p = sum(sum(Q.^2))/6;
32  sp = sqrt(p);
33  */
34  A = (R.transpose())*R;
35 
36  m = A.trace()/3;
37  Q = A-m*Matrix3::Identity(3,3);
38  q = Q.determinant()/2;
39  p=(Q.array()*Q.array()).sum()/6;
40  sp = sqrt(p);
41 
42  /* Equivalent Matlab code:
43 
44  theta = atan2(sqrt(abs(p^3-q^2)), q)/3;
45 
46  ctheta = cos(theta);
47  stheta = sin(theta);
48  */
49  theta = atan2(sqrt(abs(p*p*p-q*q)),q)/3;
50 
51  ctheta = cos(theta);
52  stheta = sin(theta);
53 
54  /* Equivalent Matlab code:
55 
56  l1 = abs(m + 2*sp*ctheta);
57  l2 = abs(m - sp*(ctheta+sqrt(3)*stheta));
58  l3 = abs(m - sp*(ctheta-sqrt(3)*stheta));
59  */
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));
63 
64  /* Equivalent Matlab code:
65 
66  a0 = sqrt(x1*x2*x3);
67  a1 = sqrt(x1*x2)+sqrt(x1*x3)+sqrt(x3*x2);
68  a2 = sqrt(x1)+sqrt(x2)+sqrt(x3);
69  */
70  a0 = sqrt(l1*l2*l3);
71  a1 = sqrt(l1*l2)+sqrt(l1*l3)+sqrt(l3*l2);
72  a2 = sqrt(l1)+sqrt(l2)+sqrt(l3);
73 
74  /* Equivalent Matlab code:
75 
76  dem = a0*(a2*a1-a0);
77 
78  b0 = (a2*a1^2 -a0*(a2^2 +a1))/dem;
79  b1 = (a0+a2*(a2^2 -2*a1))/dem;
80  b2 = a2/dem;
81  */
82  dem = a0*(a2*a1-a0);
83 
84  b0 = (a2*a1*a1 -a0*(a2*a2 +a1))/dem;
85  b1 = (a0+a2*(a2*a2-2*a1))/dem;
86  b2 = a2/dem;
87 
88  /* Equivalent Matlab code:
89 
90  U = b2*A*A -b1*A +b0*eye(3);
91 
92  X= R*U;
93  */
94  U = b2*A*A-b1*A+b0*Matrix3::Identity(3,3);
95  X = R*U;
96 }
97