| 1060 | |
| 1061 | template <typename PointType> |
| 1062 | void KD_TREE<PointType>::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) |
| 1063 | { |
| 1064 | if (root == nullptr || root->tree_deleted) |
| 1065 | return; |
| 1066 | float cur_dist = calc_box_dist(root, point); |
| 1067 | float max_dist_sqr = max_dist * max_dist; |
| 1068 | if (cur_dist > max_dist_sqr) |
| 1069 | return; |
| 1070 | int retval; |
| 1071 | if (root->need_push_down_to_left || root->need_push_down_to_right) |
| 1072 | { |
| 1073 | retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); |
| 1074 | if (retval == 0) |
| 1075 | { |
| 1076 | Push_Down(root); |
| 1077 | pthread_mutex_unlock(&(root->push_down_mutex_lock)); |
| 1078 | } |
| 1079 | else |
| 1080 | { |
| 1081 | pthread_mutex_lock(&(root->push_down_mutex_lock)); |
| 1082 | pthread_mutex_unlock(&(root->push_down_mutex_lock)); |
| 1083 | } |
| 1084 | } |
| 1085 | if (!root->point_deleted) |
| 1086 | { |
| 1087 | float dist = calc_dist(point, root->point); |
| 1088 | if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) |
| 1089 | { |
| 1090 | if (q.size() >= k_nearest) |
| 1091 | q.pop(); |
| 1092 | PointType_CMP current_point{root->point, dist}; |
| 1093 | q.push(current_point); |
| 1094 | } |
| 1095 | } |
| 1096 | int cur_search_counter; |
| 1097 | float dist_left_node = calc_box_dist(root->left_son_ptr, point); |
| 1098 | float dist_right_node = calc_box_dist(root->right_son_ptr, point); |
| 1099 | if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) |
| 1100 | { |
| 1101 | if (dist_left_node <= dist_right_node) |
| 1102 | { |
| 1103 | if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) |
| 1104 | { |
| 1105 | Search(root->left_son_ptr, k_nearest, point, q, max_dist); |
| 1106 | } |
| 1107 | else |
| 1108 | { |
| 1109 | pthread_mutex_lock(&search_flag_mutex); |
| 1110 | while (search_mutex_counter == -1) |
| 1111 | { |
| 1112 | pthread_mutex_unlock(&search_flag_mutex); |
| 1113 | usleep(1); |
| 1114 | pthread_mutex_lock(&search_flag_mutex); |
| 1115 | } |
| 1116 | search_mutex_counter += 1; |
| 1117 | pthread_mutex_unlock(&search_flag_mutex); |
| 1118 | Search(root->left_son_ptr, k_nearest, point, q, max_dist); |
| 1119 | pthread_mutex_lock(&search_flag_mutex); |