Scalar minDistanceSquared( const Gaussian<Scalar, NDims>& model, const Matrix<Scalar, NDims, Dynamic>& bag, int* index ) { Scalar minDistance = numeric_limits<Scalar>::infinity(); int minIndex = -1; for (int p = 0; p < bag.cols(); p++) { Matrix<Scalar, NDims, 1> point(bag.col(p)); Scalar distance = model.distanceSquared(point); if (distance < minDistance) { minDistance = distance; minIndex = p; } } if (index) { *index = minIndex; } return minDistance; }