Fixed the new math.  It is now correct but not optimally fast because of calls to numpy.