Extract math methods

Extract math and orientation related code