Refactoring, essentially extracted a few methods, such as vector3 cross product and subtraction.
Also, added debug code, to be removed later.

Haven't been able to test, since elm runs out of memory at 4GB.  Was hoping refactoring might avoid this, it seems quite temperamental.