VCG LibraryでkdTree

#include <vcg/space/index/kdtree/kdtree.h>

auto itr = vcg::tri::Allocator<MyMesh>::AddVertices(mesh, 30000);
for (size_t i = 0; i < 30000; i++) {
  double k1 = rand() / (double)RAND_MAX;
  double k2 = rand() / (double)RAND_MAX;
  double k3 = rand() / (double)RAND_MAX;
  itr[i].P() = vcg::Point3f(k1, k2, k3);
  itr[i].dist = 0.6;
}

// 検索して得られた頂点IDを格納する
std::vector<unsigned int> vpid;

// 検索して得られた頂点の、指定した座標からの距離を格納する
std::vector<float> vpdst;

vcg::VertexConstDataWrapper<MyMesh> ww(mesh);
vcg::KdTree<float> tree(ww);

//検索する座標 p を定義
vcg::Point3f p(0.5, 0.5, 0.5);

double distance = 0.5;
//検索
tree.doQueryDist(p, distance, vpid, vpdst);

//検索して得られた頂点に、pとの二乗距離を格納（distメンバ変数は自分で定義したもの）
for(int i = 0; i < vpid.size();i++){
  mesh.vert[ vpid[i] ].dist = vpdst[i];
}

/// Squared euclidian normal
inline T SquaredNorm() const
{
  return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}