Refactor the code using Eigen to get rid of both Geometry::indexOfMinimum
Use comma initializer followed by finished() in distanceSegmentTriangle