| 39 |
|
* such damages. |
| 40 |
|
*/ |
| 41 |
|
#include <algorithm> |
| 42 |
+ |
#include <math.h> |
| 43 |
|
#include "primitives/RigidBody.hpp" |
| 44 |
|
#include "utils/simError.h" |
| 45 |
|
namespace oopse { |
| 174 |
|
Itmp(2, 2) += mtmp * r2; |
| 175 |
|
} |
| 176 |
|
|
| 177 |
+ |
//project the inertial moment of directional atoms into this rigid body |
| 178 |
+ |
for (std::size_t i = 0; i < atoms_.size(); i++) { |
| 179 |
+ |
if (atoms_[i]->isDirectional()) { |
| 180 |
+ |
RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI(); |
| 181 |
+ |
Itmp(0, 0) += Iproject(0, 0); |
| 182 |
+ |
Itmp(1, 1) += Iproject(1, 1); |
| 183 |
+ |
Itmp(2, 2) += Iproject(2, 2); |
| 184 |
+ |
} |
| 185 |
+ |
} |
| 186 |
+ |
|
| 187 |
|
//diagonalize |
| 188 |
|
Vector3d evals; |
| 189 |
|
Mat3x3d::diagonalize(Itmp, evals, sU_); |
