Vector2s RigidBody::computeCenterOfMass( const VectorXs& vertices, const VectorXs& masses ) const { assert( vertices.size()%2 == 0 ); assert( 2*masses.size() == vertices.size() ); // TODO: By using a partial reduction from Eigen, we could make this a single line of vectorized code :). Vector2s cm(0.0,0.0); for( int i = 0; i < vertices.size()/2; ++i ) cm += masses(i)*vertices.segment<2>(2*i); scalar M = masses.sum(); assert( M > 0.0 ); cm /= M; return cm; }
scalar RigidBody::computeTotalMass( const VectorXs& masses ) const { return masses.sum(); }