CayleyMethod.cpp
Go to the documentation of this file.
1 #include "methods.h"
2 #include "util.h"
3 
12 {
13  Matrix4 U;
14  Vector4 q;
15  MatrixXf::Index index; // finding maximum value index of sum(U.^2)
16 
17  /* Equivalent Matlab code:
18 
19  U = [1+R(1,1)+R(2,2)+R(3,3) R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2) ;
20  R(3,2)-R(2,3) 1+R(1,1)-R(2,2)-R(3,3) R(1,2)+R(2,1) R(3,1)+R(1,3) ;
21  R(1,3)-R(3,1) R(1,2)+R(2,1) 1-R(1,1)+R(2,2)-R(3,3) R(2,3)+R(3,2) ;
22  R(2,1)-R(1,2) R(3,1)+R(1,3) R(2,3)+R(3,2) 1-R(1,1)-R(2,2)+R(3,3)];
23  */
24  U(0,0) = 1+R(0,0)+R(1,1)+R(2,2);
25  U(0,1) = R(2,1)-R(1,2);
26  U(0,2) = R(0,2)-R(2,0);
27  U(0,3) = R(1,0)-R(0,1);
28 
29  U(1,0) = U(0,1);
30  U(1,1) = 1+R(0,0)-R(1,1)-R(2,2);
31  U(1,2) = R(0,1)+R(1,0);
32  U(1,3) = R(2,0)+R(0,2);
33 
34  U(2,0) = U(0,2);
35  U(2,1) = U(1,2);
36  U(2,2) = 1-R(0,0)+R(1,1)-R(2,2);
37  U(2,3) = R(1,2)+R(2,1);
38 
39  U(3,0) = U(0,3);
40  U(3,1) = U(1,3);
41  U(3,2) = U(2,3);
42  U(3,3) = 1-R(0,0)-R(1,1)+R(2,2);
43 
44  /* Equivalent Matlab code:
45 
46  q = vecnorm(A,2,2);
47  */
48  q=U.rowwise().norm();
49 
50  /* Equivalent Matlab code:
51 
52  [~, index] = max(q);
53  */
54  q.maxCoeff(&index);
55 
56  switch (index)
57  {
58  case 0:
59  /* Equivalent Matlab code:
60  q(1) = q(1);
61  q(2) = sign(U(1,2))*q(2);
62  q(3) = sign(U(1,3))*q(3);
63  q(4) = sign(U(1,4))*q(4);
64  */
65  q(0) = q(0);
66  q(1) = sign(U(0,1))*q(1);
67  q(2) = sign(U(0,2))*q(2);
68  q(3) = sign(U(0,3))*q(3);
69  break;
70 
71  case 1:
72  /* Equivalent Matlab code:
73  q(1) = sign(U(2,1))*q(1);
74  q(2) = q(2);
75  q(3) = sign(U(2,3))*q(3);
76  q(4) = sign(U(2,4))*q(4);
77  */
78  q(0) = sign(U(1,0))*q(0);
79  q(1) = q(1);
80  q(2) = sign(U(1,2))*q(2);
81  q(3) = sign(U(1,3))*q(3);
82  break;
83 
84  case 2:
85  /* Equivalent Matlab code:
86  q(1) = sign(U(3,1))*q(1);
87  q(2) = sign(U(3,2))*q(2);
88  q(3) = q(3);
89  q(4) = sign(U(3,4))*q(4);
90  */
91  q(0) = sign(U(2,0))*q(0);
92  q(1) = sign(U(2,1))*q(1);
93  q(2) = q(2);
94  q(3) = sign(U(2,3))*q(3);
95  break;
96 
97  case 3:
98  /* Equivalent Matlab code:
99  q(1) = sign(U(4,1))*q(1);
100  q(2) = sign(U(4,2))*q(2);
101  q(3) = sign(U(4,3))*q(3);
102  q(4) = q(4);
103  */
104  q(0) = sign(U(3,0))*q(0);
105  q(1) = sign(U(3,1))*q(1);
106  q(2) = sign(U(3,2))*q(2);
107  q(3) = q(3);
108  }
109 
110  /* Equivalent Matlab code:
111  X = Quat2Mat(q);
112  */
113  Quat2Mat(X,q);
114 }