Added a function find the nearest vertex to a point, and also fixed some stupid bugs.