Fixed a few more math bugs.  The math is now tested and correct.  Also did a little optimization.