| 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 |
| 199 |
|
Real ad1, ad2, ad3; |
| 200 |
|
t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0; |
| 201 |
|
|
| 202 |
< |
if( t > 0.0 ){ |
| 202 |
> |
if( t > NumericConstant::epsilon ){ |
| 203 |
|
|
| 204 |
|
s = 0.5 / sqrt( t ); |
| 205 |
|
q[0] = 0.25 / s; |
| 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. |
| 352 |
|
* The eigenvectors (the columns of V) will be normalized. |
