draw floor plane, fixed a few bugs discovered from floor plane error