some optimization to RigidBodyController.cs