Modified lots of the 2D code so that it should work 3D, need to fix some bugs.