Fixed template instatiation errors and other minor refactors.
In progress: octree construction. Fixing template instantiation problems after added all code to construction methods.