About code rewrite

Some general things to consider for the rewrite.

About pointers

I see you had problems with memory leaks. having the count++ might have helped to solve the problem. (you are using c++0x) starting from c++11 there is the concept of shared_ptr and the concept of unique_ptr, weak_ptr etc and have integrated garbage collection on pointers

So if you are using somewhere something like:

vector< *T >  my_vec;

then a change to share_ptr or unique_ptr would be useful.

vector< shared_ptr< T > > v;

boost.geometry

Has a lot of algorithms. I ask because of Vector3. In Vector3 class you could add the boost algorithms that apply to your problem, kind of wrapping what its needed.