-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathSO3Methods.cpp
More file actions
executable file
·103 lines (102 loc) · 2.79 KB
/
Copy pathSO3Methods.cpp
File metadata and controls
executable file
·103 lines (102 loc) · 2.79 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#include "SO3Methods.h"
//arma::mat getRotMat(const arma::colvec& Omega)
//{
// double Om = arma::norm(Omega);
// if (Om < SO3M_EPSILON) {
// return arma::eye(3,3);
// }
// double cosOm = std::cos(Om);
// double sinOm = std::sin(Om);
// double Omsq = Om*Om;
// double fac1 = (1-cosOm)/Omsq;
// double fac2 = sinOm/Om;
// arma::mat R = arma::zeros(3,3);
// R(0,0) = cosOm+Omega(0)*Omega(0)*fac1;
// R(1,1) = cosOm+Omega(1)*Omega(1)*fac1;
// R(2,2) = cosOm+Omega(2)*Omega(2)*fac1;
// double A = Omega(0)*Omega(1)*fac1;
// double B = Omega(2)*fac2;
// R(0,1) = A-B;
// R(1,0) = A+B;
// A = Omega(0)*Omega(2)*fac1;
// B = Omega(1)*fac2;
// R(0,2) = A+B;
// R(2,0) = A-B;
// A = Omega(1)*Omega(2)*fac1;
// B = Omega(0)*fac2;
// R(1,2) = A-B;
// R(2,1) = A+B;
// return R;
//}
//
//arma::colvec ExtractTheta(const arma::mat& R) {
// double val = 0.5* (arma::trace(R)-1);
// if (val > S03M_CLOSE_TO_ONE) {
// return arma::zeros(3);
// }
// if (val < S03M_CLOSE_TO_MINUS_ONE) {
// if (R(0,0) > S03M_CLOSE_TO_ONE) {
// return {M_PI,0,0};
// }
// if (R(1,1) > S03M_CLOSE_TO_ONE) {
// return {0,M_PI,0};
// }
// return {0,0,M_PI};
// }
// double Th = std::acos(val);
// arma::colvec Theta = {(R(2,1)-R(1,2)),(R(0,2)-R(2,0)),(R(1,0)-R(0,1))};
// Theta = Th*0.5/std::sin(Th) * Theta;
// return Theta;
//}
//
//double ExtractTheta1(const arma::mat& R) {
// double val = 0.5* (arma::trace(R)-1);
// if (val > S03M_CLOSE_TO_ONE) {
// return 0;
// }
// if (val < S03M_CLOSE_TO_MINUS_ONE) {
// if (R(0,0) > S03M_CLOSE_TO_ONE) {
// return M_PI;
// }
// return 0;
// }
// double Th = std::acos(val);
// return Th*0.5/std::sin(Th) * (R(2,1)-R(1,2));
//}
//
//double ExtractTheta2(const arma::mat& R) {
// double val = 0.5* (arma::trace(R)-1);
// if (val > S03M_CLOSE_TO_ONE) {
// return 0;
// }
// if (val < S03M_CLOSE_TO_MINUS_ONE) {
// if (R(1,1) > S03M_CLOSE_TO_ONE) {
// return M_PI;
// }
// return 0;
// }
// double Th = std::acos(val);
// return Th*0.5/std::sin(Th) * (R(0,2)-R(2,0));
//}
//
//double ExtractTheta3(const arma::mat& R) {
// double val = 0.5* (arma::trace(R)-1);
// if (val > S03M_CLOSE_TO_ONE) {
// return 0;
// }
// if (val < S03M_CLOSE_TO_MINUS_ONE) {
// if (R(2,2) > S03M_CLOSE_TO_ONE) {
// return M_PI;
// }
// return 0;
// }
// double Th = std::acos(val);
// return Th*0.5/std::sin(Th)* (R(1,0)-R(0,1));
//}
//
//arma::mat Rotz(const double& theta) {
// double c = std::cos(theta);
// double s = std::sin(theta);
// arma::mat Rz = {{c,-s,0},{s,c,0},{0,0,1}};
// return Rz;
//}