RigidBody::RigidBody( const Vector2s& v, const scalar& omega, const VectorXs& vertices, const VectorXs& masses, const scalar& radius )
: m_M(-1.0)
//, m_masses(masses)
, m_I(-1.0)
, m_vertices(vertices)
, m_r(radius)
, m_X(-1.0,-1.0)
, m_theta(0.0)
, m_V(v)
, m_omega(omega)
// theta == 0 => identity matrix
, m_R(Matrix2s::Identity())
, m_F(0.0,0.0)
, m_tau(0.0)
{
m_M = computeTotalMass(masses);
m_X = computeCenterOfMass(vertices,masses);
m_I = computeMomentOfInertia(vertices,masses);
assert( (masses.array()>0.0).all() );
assert( m_M > 0.0 );
assert( m_I > 0.0 );
assert( m_vertices.size() >= 2 );
assert( m_r >= 0.0 );
// Translate the rigid body so the body space center of mass is the origin
for( int i = 0; i < m_vertices.size()/2; ++i ) m_vertices.segment<2>(2*i) -= m_X;
}
double computeTotalMass(taoDNode * node)
{
double mass(0);
if (node->mass()) {
// I guess TAO nodes always have a mass, but the interface
// returns a pointer, so maybe there are cases where there is
// not even a zero mass? Whatever, just be paranoid and check
// for non-NULL pointers.
mass = *node->mass();
}
for (taoDNode * child(node->getDChild()); child != NULL; child = child->getDSibling()) {
mass += computeTotalMass(child);
}
return mass;
}
