166 |
|
void setupRotMat(Real w, Real x, Real y, Real z) { |
167 |
|
Quaternion<Real> q(w, x, y, z); |
168 |
|
*this = q.toRotationMatrix3(); |
169 |
+ |
} |
170 |
+ |
|
171 |
+ |
void setupSkewMat(Vector3<Real> v) { |
172 |
+ |
setupSkewMat(v[0], v[1], v[2]); |
173 |
+ |
} |
174 |
+ |
|
175 |
+ |
void setupSkewMat(Real v1, Real v2, Real v3) { |
176 |
+ |
this->data_[0][0] = 0; |
177 |
+ |
this->data_[0][1] = -v3; |
178 |
+ |
this->data_[0][2] = v2; |
179 |
+ |
this->data_[1][0] = v3; |
180 |
+ |
this->data_[1][1] = 0; |
181 |
+ |
this->data_[1][2] = -v1; |
182 |
+ |
this->data_[2][0] = -v2; |
183 |
+ |
this->data_[2][1] = v1; |
184 |
+ |
this->data_[2][2] = 0; |
185 |
+ |
|
186 |
+ |
|
187 |
|
} |
188 |
|
|
189 |
+ |
|
190 |
+ |
|
191 |
|
/** |
192 |
|
* Returns the quaternion from this rotation matrix |
193 |
|
* @return the quaternion from this rotation matrix |
208 |
|
q[3] = (this->data_[0][1] - this->data_[1][0]) * s; |
209 |
|
} else { |
210 |
|
|
211 |
< |
ad1 = fabs( this->data_[0][0] ); |
212 |
< |
ad2 = fabs( this->data_[1][1] ); |
213 |
< |
ad3 = fabs( this->data_[2][2] ); |
211 |
> |
ad1 = this->data_[0][0]; |
212 |
> |
ad2 = this->data_[1][1]; |
213 |
> |
ad3 = this->data_[2][2]; |
214 |
|
|
215 |
|
if( ad1 >= ad2 && ad1 >= ad3 ){ |
216 |
|
|
336 |
|
|
337 |
|
m /= det; |
338 |
|
return m; |
339 |
+ |
} |
340 |
+ |
|
341 |
+ |
SquareMatrix3<Real> transpose() const{ |
342 |
+ |
SquareMatrix3<Real> result; |
343 |
+ |
|
344 |
+ |
for (unsigned int i = 0; i < 3; i++) |
345 |
+ |
for (unsigned int j = 0; j < 3; j++) |
346 |
+ |
result(j, i) = this->data_[i][j]; |
347 |
+ |
|
348 |
+ |
return result; |
349 |
|
} |
350 |
|
/** |
351 |
|
* Extract the eigenvalues and eigenvectors from a 3x3 matrix. |