Fix some errors made during Matlab translation.
First working version using automatic triangulation?