The code is too complicated for me to easily see where the calculations are incorrect. Perhaps a test case in 2D would be easier to (visually) debug.
