Added integration function for RigidBody