Completed DiagMotion

Hey Matt, I believe that I have completed DiagMotion. I haven't tested,
but conceptually it should work... if I made no typos or logical slips.
It also looks a lot cleaner. :)