void Molecule::moveToPAF()
{
Eigen::Matrix3d inertia = inertiaTensor();
// Diagonalize inertia tensor V^t * I * V
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
if (eigenSolver.info() != Eigen::Success) abort();
// Rotate to Principal Axes Frame
this->rotate(eigenSolver.eigenvectors());
std::cout << eigenSolver.eigenvalues() << std::endl;
}
rotorType Molecule::findRotorType()
{
rotorType type;
if (nAtoms_ == 1) {
type = rtAtom;
} else {
// Get inertia tensor
Eigen::Matrix3d inertia = inertiaTensor();
// Diagonalize inertia tensor V^t * I * V
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
if (eigenSolver.info() != Eigen::Success) abort();
// Determine the degeneracy of the eigenvalues.
int deg = 0;
double tmp, abs, rel;
for (int i = 0; i < 2; ++i) {
for (int j = i + 1; j < 3 && deg < 2; ++j) { // Check i and j != i
abs = fabs(eigenSolver.eigenvalues()[i] - eigenSolver.eigenvalues()[j]);
tmp = eigenSolver.eigenvalues()[j]; // Because the eigenvalues are already in ascending order.
if (abs > 1.0e-14) {
rel = abs/tmp;
} else {
rel = 0.0;
}
if (rel < 1.0e-8) {
++deg;
}
}
}
// Get the rotor type based on the degeneracy.
if (eigenSolver.eigenvalues()[0] == 0.0) {
type = rtLinear;
} else if (deg == 2) {
type = rtSpherical;
} else if (deg == 1) { // We do not distinguish between prolate and oblate.
type = rtSymmetric;
} else {
type = rtAsymmetric;
}
}
return type;
}
dgFloat32 dgCollisionCone::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
dgFloat32 volume;
dgFloat32 inertaxx;
dgFloat32 inertayyzz;
//dgVector centerOfMass1;
//dgVector inertia1;
//dgVector crossInertia1;
//volume = dgCollisionConvex::CalculateMassProperties (inertia1, crossInertia1, centerOfMass1);
volume = dgFloat32 (3.1616f * 2.0f / 3.0f) * m_radius * m_radius * m_height;
centerOfMass = GetOffsetMatrix().m_posit - GetOffsetMatrix().m_front.Scale (dgFloat32 (0.5f) * m_height);
inertaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius * volume;
inertayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (4.0f / 10.0f) * m_height * m_height) * volume;
dgMatrix inertiaTensor (dgGetIdentityMatrix());
inertiaTensor[0][0] = inertaxx;
inertiaTensor[1][1] = inertayyzz;
inertiaTensor[2][2] = inertayyzz;
inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();
crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;
dgVector central (centerOfMass.CompProduct(centerOfMass));
inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);
centerOfMass = centerOfMass.Scale (volume);
return volume;
}
void Object::applyAngularImpulse() {
if (!_fixed)
_angularVelocity += glm::inverse(inertiaTensor())*_angularImpulse;
}
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
btMatrix3x3 linkInertiaBasis;
btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
linkMass = 0.f;
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
else
{
linkMass = link->m_inertia.m_mass;
if (link->m_inertia.m_ixy == 0.0 &&
link->m_inertia.m_ixz == 0.0 &&
link->m_inertia.m_iyz == 0.0)
{
principalInertiaX = link->m_inertia.m_ixx;
principalInertiaY = link->m_inertia.m_iyy;
principalInertiaZ = link->m_inertia.m_izz;
linkInertiaBasis.setIdentity();
}
else
{
principalInertiaX = link->m_inertia.m_ixx;
btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btScalar threshold = 1.0e-6;
int numIterations = 30;
inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
principalInertiaX = inertiaTensor[0][0];
principalInertiaY = inertiaTensor[1][1];
principalInertiaZ = inertiaTensor[2][2];
}
}
mass = linkMass;
if (principalInertiaX < 0 ||
principalInertiaX > (principalInertiaY + principalInertiaZ) ||
principalInertiaY < 0 ||
principalInertiaY > (principalInertiaX + principalInertiaZ) ||
principalInertiaZ < 0 ||
principalInertiaZ > (principalInertiaX + principalInertiaY))
{
b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
