Physics engine debug.

Fixed some geometry bugs.