| 17 | } |
| 18 | |
| 19 | void PointLocator::locate(const MatrixFr& points) { |
| 20 | const Float eps = 1e-6; |
| 21 | const size_t num_pts = points.rows(); |
| 22 | m_voxel_idx = VectorI::Zero(num_pts); |
| 23 | m_barycentric_coords = MatrixFr::Zero(num_pts, m_vertex_per_element); |
| 24 | |
| 25 | for (size_t i=0; i<num_pts; i++) { |
| 26 | VectorF v = points.row(i); |
| 27 | VectorI candidate_elems = m_grid->get_items_near_point(v); |
| 28 | |
| 29 | VectorF barycentric_coord; |
| 30 | VectorF best_barycentric_coord; |
| 31 | |
| 32 | bool found = false; |
| 33 | Float least_negative_coordinate = -std::numeric_limits<Float>::max(); |
| 34 | const size_t num_candidates = candidate_elems.size(); |
| 35 | for (size_t j=0; j<num_candidates; j++) { |
| 36 | barycentric_coord = compute_barycentric_coord( |
| 37 | v, candidate_elems[j]); |
| 38 | Float min_barycentric_coord = barycentric_coord.minCoeff(); |
| 39 | if (min_barycentric_coord > least_negative_coordinate) { |
| 40 | found = true; |
| 41 | least_negative_coordinate = min_barycentric_coord; |
| 42 | m_voxel_idx[i] = candidate_elems[j]; |
| 43 | best_barycentric_coord = barycentric_coord; |
| 44 | if (min_barycentric_coord >= -eps) { |
| 45 | break; |
| 46 | } |
| 47 | } |
| 48 | } |
| 49 | |
| 50 | if (!found) { |
| 51 | std::stringstream err_msg; |
| 52 | err_msg << "Point ( "; |
| 53 | for (size_t i=0; i<m_mesh->get_dim(); i++) { |
| 54 | err_msg << v[i] << " "; |
| 55 | } |
| 56 | err_msg << ") is not inside of any voxels" << std::endl; |
| 57 | throw RuntimeError(err_msg.str()); |
| 58 | } |
| 59 | |
| 60 | m_barycentric_coords.row(i) = best_barycentric_coord; |
| 61 | } |
| 62 | } |
| 63 | |
| 64 | void PointLocator::clear() { |
| 65 | m_voxel_idx.resize(0); |