Cleaned up some code and added a RigidBody class to help smooth orientation.