Updated rigid body generalized methods to use VectorNd/MatrixNd