Merge pull request #547 from pusewicz/fix-math-correctness