void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize)
{
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
}
void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int qsize)
{
Triangle last_tri1 = node->tri_indices1[node->last_tri_id1];
Triangle last_tri2 = node->tri_indices2[node->last_tri_id2];
Vec3f last_tri1_points[3];
Vec3f last_tri2_points[3];
last_tri1_points[0] = node->vertices1[last_tri1[0]];
last_tri1_points[1] = node->vertices1[last_tri1[1]];
last_tri1_points[2] = node->vertices1[last_tri1[2]];
last_tri2_points[0] = node->vertices2[last_tri2[0]];
last_tri2_points[1] = node->vertices2[last_tri2[1]];
last_tri2_points[2] = node->vertices2[last_tri2[2]];
Vec3f last_tri_P, last_tri_Q;
node->min_distance = TriangleDistance::triDistance(last_tri1_points[0], last_tri1_points[1], last_tri1_points[2],
last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
node->R, node->T, last_tri_P, last_tri_Q);
node->p1 = last_tri_P;
node->p2 = matTransMulVec(node->R, last_tri_Q - node->T);
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
Vec3f u = node->p2 - node->T;
node->p2 = matTransMulVec(node->R, u);
}
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize)
{
node->preprocess();
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
node->postprocess();
}
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize)
{
BVTQ bvtq;
bvtq.qsize = qsize;
BVT min_test;
min_test.b1 = b1;
min_test.b2 = b2;
while(1)
{
bool l1 = node->isFirstNodeLeaf(min_test.b1);
bool l2 = node->isSecondNodeLeaf(min_test.b2);
if(l1 && l2)
{
updateFrontList(front_list, min_test.b1, min_test.b2);
node->leafTesting(min_test.b1, min_test.b2);
}
else if(bvtq.full())
{
// queue should not get two more tests, recur
distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize);
}
else
{
// queue capacity is not full yet
BVT bvt1, bvt2;
if(node->firstOverSecond(min_test.b1, min_test.b2))
{
int c1 = node->getFirstLeftChild(min_test.b1);
int c2 = node->getFirstRightChild(min_test.b1);
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
}
else
{
int c1 = node->getSecondLeftChild(min_test.b2);
int c2 = node->getSecondRightChild(min_test.b2);
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
}
bvtq.push(bvt1);
bvtq.push(bvt2);
}
if(bvtq.empty())
break;
else
{
min_test = bvtq.top();
bvtq.pop();
if(node->canStop(min_test.d))
{
updateFrontList(front_list, min_test.b1, min_test.b2);
break;
}
}
}
}
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int bv_node1_id, int bv_node2_id, BVHFrontList* front_list, int qsize)
{
BVTQ bvtq;
bvtq.qsize = qsize;
BVT min_test;
min_test.bv_node1_id = bv_node1_id;
min_test.bv_node2_id = bv_node2_id;
FCL_REAL whole_distance_fraction = node->whole_distance_ / 100.0;
while(1)
{
bool l1 = node->isFirstNodeLeaf(min_test.bv_node1_id);
bool l2 = node->isSecondNodeLeaf(min_test.bv_node2_id);
if(l1 && l2)
{
updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id);
node->leafTesting(min_test.bv_node1_id, min_test.bv_node2_id);
}
else if(bvtq.full())
{
// queue should not get two more tests, recur
distanceQueueRecurse(node, min_test.bv_node1_id, min_test.bv_node2_id, front_list, qsize);
}
else
{
// queue capacity is not full yet
BVT bvt1, bvt2;
if(node->firstOverSecond(min_test.bv_node1_id, min_test.bv_node2_id))
{
int c1 = node->getFirstLeftChild(min_test.bv_node1_id);
int c2 = node->getFirstRightChild(min_test.bv_node1_id);
bvt1.bv_node1_id = c1;
bvt1.bv_node2_id = min_test.bv_node2_id;
bvt1.d = node->BVTesting(bvt1.bv_node1_id, bvt1.bv_node2_id);
bvt2.bv_node1_id = c2;
bvt2.bv_node2_id = min_test.bv_node2_id;
bvt2.d = node->BVTesting(bvt2.bv_node1_id, bvt2.bv_node2_id);
}
else
{
int c1 = node->getSecondLeftChild(min_test.bv_node2_id);
int c2 = node->getSecondRightChild(min_test.bv_node2_id);
bvt1.bv_node1_id = min_test.bv_node1_id;
bvt1.bv_node2_id = c1;
bvt1.d = node->BVTesting(bvt1.bv_node1_id, bvt1.bv_node2_id);
bvt2.bv_node1_id = min_test.bv_node1_id;
bvt2.bv_node2_id = c2;
bvt2.d = node->BVTesting(bvt2.bv_node1_id, bvt2.bv_node2_id);
}
bvt1.depth = min_test.depth + 1;
bvt2.depth = min_test.depth + 1;
bvtq.push(bvt1);
bvtq.push(bvt2);
}
if(bvtq.empty())
break;
else
{
min_test = bvtq.top();
bvtq.pop();
if (node->canStop(min_test.d) )
{
updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id);
break;
}
if (
min_test.d >= node->can_stop_distance_
|| (min_test.depth <= 2 && min_test.d >= whole_distance_fraction)
)
{
node->result->update(min_test.d, node->o1, node->o2, min_test.bv_node1_id, min_test.bv_node2_id);
updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id);
break;
}
}
}
}
