Added orthogonalize function

Code is based on Potlatch2's Quadrilateralise class