Added some utility functions to TriangleMesh, thanks to @alexrj Some porting to C++ based on work by @alexrj.