// 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);
}
}
}
}
}
}