Update RigidBodySimulator.cpp