Added orthogonalize function (square corners)