inline ErrorCode SpatialLocator::locate_point(const double *pos,
EntityHandle &ent, double *params, int *is_inside,
const double rel_iter_tol, const double abs_iter_tol,
const double inside_tol)
{
return locate_points(pos, 1, &ent, params, is_inside, rel_iter_tol, abs_iter_tol, inside_tol);
}
// フツーのk-NN search。
void locate_points(typename KDTree<T>::ResultQueue* pqueue, KDTreeNode* node, typename KDTree<T>::Query &query) {
if (node == NULL)
return;
const int axis = node->axis;
double delta;
switch (axis) {
case 0: delta = query.search_position.x - node->point->position.x; break;
case 1: delta = query.search_position.y - node->point->position.y; break;
case 2: delta = query.search_position.z - node->point->position.z; break;
}
// 対象点<->探索中心の距離が設定半径以下 かつ 対象点<->探索中心の法線方向の距離が一定以下 という条件ならその対象点格納
const Vec dir = node->point->position - query.search_position;
const double distance2 = dir.LengthSquared();
const double dt = Dot(query.normal, dir / sqrt(distance2));
if (distance2 < query.max_distance2 && fabs(dt) <= query.max_distance2 * 0.01) {
pqueue->push(ElementForQueue(node->point, distance2));
if (pqueue->size() > query.max_search_num) {
pqueue->pop();
query.max_distance2 = pqueue->top().distance2;
}
}
if (delta > 0.0) { // みぎ
locate_points(pqueue,node->right, query);
if (delta * delta < query.max_distance2) {
locate_points(pqueue, node->left, query);
}
} else { // ひだり
locate_points(pqueue,node->left, query);
if (delta * delta < query.max_distance2) {
locate_points(pqueue, node->right, query);
}
}
}
void SearchKNN(typename KDTree::ResultQueue* pqueue, typename KDTree<T>::Query &query) {
locate_points(pqueue, root, query);
}