Decompose given matrix.
53 {
55
56
57
58 B.a00 = A.a00 * A.a00 + A.a10 * A.a10 + A.a20 * A.a20;
59
60 B.a10 = A.a01 * A.a00 + A.a11 * A.a10 + A.a21 * A.a20;
61 B.a11 = A.a01 * A.a01 + A.a11 * A.a11 + A.a21 * A.a21;
62
63 B.a20 = A.a02 * A.a00 + A.a12 * A.a10 + A.a22 * A.a20;
64 B.a21 = A.a02 * A.a01 + A.a12 * A.a11 + A.a22 * A.a21;
65 B.a22 = A.a02 * A.a02 + A.a12 * A.a12 + A.a22 * A.a22;
66
67 B.a01 = B.a10;
68 B.a02 = B.a20;
69 B.a12 = B.a21;
70
71
72
73
74 const JQuaternion q(B);
75
76 const double w = q[3];
77 const double x = q[0];
78 const double y = q[1];
79 const double z = q[2];
80
81 const double xx =
x*
x;
82 const double yy =
y*
y;
83 const double zz = z*z;
84 const double xz =
x*z;
85 const double xy =
x*
y;
86 const double yz =
y*z;
87 const double wx = w*
x;
88 const double wy = w*
y;
89 const double wz = w*z;
90
91 V.
a00 = 1.0 - 2.0*(yy + zz);
V.
a01 = 2.0*(xy - wz);
V.
a02 = 2.0*(xz + wy);
92 V.
a10 = 2.0*(xy + wz);
V.
a11 = 1.0 - 2.0*(xx + zz);
V.
a12 = 2.0*(yz - wx);
93 V.
a20 = 2.0*(xz - wy);
V.
a21 = 2.0*(yz + wx);
V.
a22 = 1.0 - 2.0*(xx + yy);
94
95
97
98
99
100
104
105 if (rho1 < rho2) {
109 swap(rho1,rho2);
110 }
111
112 if (rho1 < rho3) {
116 swap(rho1,rho3);
117 }
118
119 if (rho2 < rho3) {
123 }
124
125
126
127
130
132
133 const JGivens q1(B.a00, B.a10);
134
136 b = q1.getb();
137
138
139
140 R.a00 =
a*B.a00 + b*B.a10; R.a01 =
a*B.a01 + b*B.a11; R.a02 =
a*B.a02 + b*B.a12;
141 R.a10 = -b*B.a00 +
a*B.a10; R.a11 = -b*B.a01 +
a*B.a11; R.a12 = -b*B.a02 +
a*B.a12;
142 R.a20 = B.a20; R.a21 = B.a21; R.a22 = B.a22;
143
144 const JGivens q2(R.a00, R.a20);
145
147 b = q2.getb();
148
149
150
151 B.a00 =
a*R.a00 + b*R.a20; B.a01 =
a*R.a01 + b*R.a21; B.a02 =
a*R.a02 + b*R.a22;
152 B.a10 = R.a10; B.a11 = R.a11; B.a12 = R.a12;
153 B.a20 = -b*R.a00 +
a*R.a20; B.a21 = -b*R.a01 +
a*R.a21; B.a22 = -b*R.a02 +
a*R.a22;
154
155 const JGivens q3(B.a11, B.a21);
156
158 b = q3.getb();
159
160
161
162 R.a00 = B.a00; R.a01 = B.a01; R.a02 = B.a02;
163 R.a10 =
a*B.a10+b*B.a20; R.a11 =
a*B.a11 + b*B.a21; R.a12 =
a*B.a12 + b*B.a22;
164 R.a20 = -b*B.a10+
a*B.a20; R.a21 = -b*B.a11 +
a*B.a21; R.a22 = -b*B.a12 +
a*B.a22;
165
166
167
168 const double sp1 = -1.0 + 2.0*q1.sh*q1.sh;
169 const double sp2 = -1.0 + 2.0*q2.sh*q2.sh;
170 const double sp3 = -1.0 + 2.0*q3.sh*q3.sh;
171
172 Q.a00 = sp1*sp2;
173 Q.a01 = 4.0*q2.ch*q3.ch*sp1*q2.sh*q3.sh + 2.0*q1.ch*q1.sh*sp3;
174 Q.a02 = 4.0*q1.ch*q3.ch*q1.sh*q3.sh - 2.0*q2.ch*sp1*q2.sh*sp3;
175
176 Q.a10 = -2.0*q1.ch*q1.sh*sp2;
177 Q.a11 = -8.0*q1.ch*q2.ch*q3.ch*q1.sh*q2.sh*q3.sh + sp1*sp3;
178 Q.a12 = -2.0*q3.ch*q3.sh + 4.0*q1.sh*(q3.ch*q1.sh*q3.sh + q1.ch*q2.ch*q2.sh*sp3);
179
180 Q.a20 = 2.0*q2.ch*q2.sh;
181 Q.a21 = -2.0*q3.ch*sp2*q3.sh;
182 Q.a22 = sp2*sp3;
183 }
static double getLengthSquared(const double x, const double y, const double z)
Get length squared.