// do full tree traversal below iBox and make sure that all triangles are further // than box-distance-sqr void debug_check_child_tri_distances(int iBox, Vector3d p) { double fBoxDistSqr = box_distance_sqr(iBox, p); TreeTraversal t = new TreeTraversal() { NextTriangleF = (tID) => { double fTriDistSqr = MeshQueries.TriDistanceSqr(mesh, tID, p); if (fTriDistSqr < fBoxDistSqr) { if (Math.Abs(fTriDistSqr - fBoxDistSqr) > MathUtil.ZeroTolerance * 100) { Util.gBreakToDebugger(); } } } }; tree_traversal(iBox, 0, t); }
void find_nearest_tri(int iBox, Vector3d p, ref double fNearestSqr, ref int tID) { int idx = box_to_index[iBox]; if (idx < triangles_end) // triange-list case, array is [N t1 t2 ... tN] { int num_tris = index_list[idx]; for (int i = 1; i <= num_tris; ++i) { int ti = index_list[idx + i]; double fTriDistSqr = MeshQueries.TriDistanceSqr(mesh, ti, p); if (fTriDistSqr < fNearestSqr) { fNearestSqr = fTriDistSqr; tID = ti; } } } else // internal node, either 1 or 2 child boxes { int iChild1 = index_list[idx]; if (iChild1 < 0) // 1 child, descend if nearer than cur min-dist { iChild1 = (-iChild1) - 1; double fChild1DistSqr = box_distance_sqr(iChild1, p); if (fChild1DistSqr <= fNearestSqr) { find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID); } } else // 2 children, descend closest first { iChild1 = iChild1 - 1; int iChild2 = index_list[idx + 1] - 1; double fChild1DistSqr = box_distance_sqr(iChild1, p); double fChild2DistSqr = box_distance_sqr(iChild2, p); if (fChild1DistSqr < fChild2DistSqr) { if (fChild1DistSqr < fNearestSqr) { find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID); if (fChild2DistSqr < fNearestSqr) { find_nearest_tri(iChild2, p, ref fNearestSqr, ref tID); } } } else { if (fChild2DistSqr < fNearestSqr) { find_nearest_tri(iChild2, p, ref fNearestSqr, ref tID); if (fChild1DistSqr < fNearestSqr) { find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID); } } } } } }