void CBoundingVolumeTree3<BV, T, Traits, SD>::BreadthFirstSearch()
{
using namespace std;
queue< CBoundingVolumeNode3<BV, T, Traits>* > qBFS;
qBFS.push(m_Children[0]);
while(!qBFS.empty())
{
CBoundingVolumeNode3<BV, T, Traits>* pNode = qBFS.front();
std::cout<<"Adress: "<<pNode<<" Trait: "<<pNode->m_Traits.iCollision<<std::endl;
qBFS.pop();
if(!pNode->IsLeaf())
{
qBFS.push(pNode->m_Children[0]);
qBFS.push(pNode->m_Children[1]);
}//end if
}//end while
}//end BreadthFirstSearch
T CDistanceMeshPoint<T>::ComputeDistanceSqr()
{
//variable declarations and initialisations
//=========================================
//helper variable we initialize it with the maximum possible value
Real d = CMath<T>::MAXREAL;
//further helper variables
Real lowerBound = CMath<T>::MAXREAL;
Real upperBound = -CMath<T>::MAXREAL;
//In this variable we save the node of the BVH that
//is located closest to the query point
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *pBest = NULL;
//we need to count how many leaves of the
//BVH we have in our list
int nLeafCount = 0;
//the list we need for the Breadth first search
//in the tree data structure
std::list<CBoundingVolumeNode3<AABB3<T>,T,CTraits>* > lBFS;
typename std::list<CBoundingVolumeNode3<AABB3<T>,T,CTraits>* >::iterator lIter;
//initialize this list with the children of the root
for(int i=0;i< m_pBVH->GetNumChildren();i++)
lBFS.push_back(m_pBVH->GetChild(i));
//get the current size of the list
int vSize = (int)lBFS.size();
//* loop until there are only leaves in the list */
while(vSize != nLeafCount)
{
//each time initialize with zeros
nLeafCount = 0;
int j = 0;
//each time set this to the maximum value
lowerBound = CMath<T>::MAXREAL;
//a auxilliary array so that we dont have to
//calculate these values multiple times
T *dLowerBounds = new T[vSize];
/* find best upper bound */
for(lIter = lBFS.begin(); lIter != lBFS.end(); lIter++)
{
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *pNode = *lIter;
dLowerBounds[j] = pNode->GetLowerBound(m_vQuery);
if(lowerBound > dLowerBounds[j])
{
lowerBound = dLowerBounds[j];
pBest = pNode;
}//end if
j++;
}//end for
/* get upper bound for best element */
upperBound = pBest->GetUpperBound(m_vQuery);
//now we check every element if we can prune
//it or if it has to remain
lIter = lBFS.begin();
for(int i = 0; i < vSize; i++)
{
//get the current element
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *pNode = *lIter;
//if the current element is the best element
//we replace it by its successors and we go on
if(pNode == pBest)
{
//if we have reached the leaf
//level no more refinement is possible
if(!pNode->IsLeaf())
{
lBFS.push_back(pNode->m_Children[0]);
lBFS.push_back(pNode->m_Children[1]);
lIter = lBFS.erase(lIter);
continue;
}//end if
//the node is a leaf, so we increase the leaf count
else
{
nLeafCount++;
lIter++;
continue;
}//end else
}//end if
//we check if our upper bound on the distance
//is larger than the lower bound of the current node
//If the lower bound of the current node is smaller
//then we refine it
if(upperBound > dLowerBounds[i])
{
//is the node a leaf, then
// it can not be refined...
if(!pNode->IsLeaf())
{
lBFS.push_back(pNode->m_Children[0]);
lBFS.push_back(pNode->m_Children[1]);
lIter = lBFS.erase(lIter);
}//end if
else
{
nLeafCount++;
lIter++;
}
}//end if
//the node's lower bound is larger than
//the best upper bound, so it can be
//pruned away
else
{
//std::cout<<"this should not happen"<<std::endl;
lIter = lBFS.erase(lIter);
}//end else
}//end for
//update the the current size of the list
vSize = (int)lBFS.size();
//delete the auxilliary array, so we dont make
//a memory leak
delete[] dLowerBounds;
}//end while
//std::cout<<"leafcount: "<<nLeafCount<<std::endl;
m_Res.pNode = pBest;
//get all the triangles contained in the best node
T mindist = CMath<T>::MAXREAL;
for(int k=0;k<pBest->m_Traits.m_vTriangles.size();k++)
{
Triangle3<T> &tri3 = pBest->m_Traits.m_vTriangles[k];
CDistancePointTriangle<T> distPointTri(tri3,m_vQuery);
T dist = distPointTri.ComputeDistance();
if(dist < mindist)
{
mindist=dist;
m_Res.iTriangleID = k;
m_Res.m_vClosestPoint=distPointTri.m_vClosestPoint1;
}
}//end for k
for(lIter=lBFS.begin();lIter!=lBFS.end();lIter++)
{
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *node = *lIter;
if(node == pBest)
continue;
for(int k=0;k<node->m_Traits.m_vTriangles.size();k++)
{
Triangle3<T> &tri3 = node->m_Traits.m_vTriangles[k];
CDistancePointTriangle<T> distPointTri(tri3,m_vQuery);
T dist = distPointTri.ComputeDistance();
if(dist < mindist)
{
mindist=dist;
m_Res.pNode = node;
m_Res.iTriangleID = k;
m_Res.m_vClosestPoint = distPointTri.m_vClosestPoint1;
}
}//end for k
}//end for liter
//finally return the square root of the distance
return T(mindist);
}
T CDistanceModelPlane<T>::ComputeDistanceEps(T eps)
{
/* top-down traverse the tree
check the dist(currentNode,Plane) against eps
if dist(currentNode,Plane) < esp
if( !currentNode->isLeaf )
expandNode
else
check triangles in currentNode
store normals and closest points in vector
return minDist,penetration,contact points, normals
*/
distchecks=0;
ndistchecks=0;
adding=0;
double dTimeTraverse=0.0;
double dTimeIntersection=0.0;
CPerfTimer timer0;
std::list<CBoundingVolumeNode3<AABB3<T>,T,CTraits>* > nodes;
typename std::list<CBoundingVolumeNode3<AABB3<T>,T,CTraits>* >::iterator liter;
m_dEps = eps;
//early out test
for(int i=0;i< m_pBVH->GetNumChildren();i++)
{
//compute distance AABB-Plane
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *pNode = m_pBVH->GetChild(i);
//project point on plane
Vector3<T> PQ = pNode->GetCenter() - m_pPlane->m_vOrigin;
T sdistCenterPlane = m_pPlane->m_vNormal * PQ;
Vector3<T> closestPoint = pNode->GetCenter() - sdistCenterPlane * m_pPlane->m_vNormal;
//calculate distance from point to AABB surface
T distPlaneBox = pNode->m_BV.minDistance(closestPoint);
if(distPlaneBox > eps)
return T(-1.0);
else
nodes.push_back(pNode);
}
timer0.Start();
for(liter=nodes.begin();liter!=nodes.end();liter++)
{
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *pNode = *liter;
Traverse(pNode);
}
dTimeTraverse=timer0.GetTime();
//std::cout<<"Time traversal: "<<dTimeTraverse<<std::endl;
timer0.Start();
//check the size of the triangle vector
if(!leaves.empty())
{
//compute dist(plane,triangles)
//fill normals and points
typename std::list<CBoundingVolumeNode3<AABB3<T>,T,CTraits> *>::iterator liter = leaves.begin();
for(;liter!=leaves.end();liter++)
{
CBoundingVolumeNode3<AABB3<T>,T,CTraits> *node = *liter;
for(int k=0;k<node->m_Traits.m_vTriangles.size();k++)
{
Triangle3<T> &tri3 = node->m_Traits.m_vTriangles[k];
VECTOR3 vPoint = tri3.Get(0);
Real dist = (vPoint - m_pPlane->m_vOrigin) * m_pPlane->m_vNormal; //mindist point-plane vertex 0
for(int j=1;j<3;j++)
{
Real d = (tri3.Get(j) - m_pPlane->m_vOrigin) * m_pPlane->m_vNormal; //mindist point-plane vertex 0
if(d < dist)
{
dist = d;
vPoint = tri3.Get(j);
}
}//end for j
if(dist < m_dEps)
m_vPoint.push_back(vPoint);
}//end for k
}//end for liter
}//end if
//if(!m_vTriangles.empty())
//{
// //compute dist(plane,triangles)
// //fill normals and points
// typename std::vector<CTriangle3<T> >::iterator titer = m_vTriangles.begin();
// for(;titer!=m_vTriangles.end();titer++)
// {
// CTriangle3<T> &tri3 = *titer;
// VECTOR3 vPoint = tri3.Get(0);
// Real dist = (vPoint - m_pPlane->m_vOrigin) * m_pPlane->m_vNormal; //mindist point-plane vertex 0
// for(int j=1;j<3;j++)
// {
// Real d = (tri3.Get(j) - m_pPlane->m_vOrigin) * m_pPlane->m_vNormal; //mindist point-plane vertex 0
// if(d < dist)
// {
// dist = d;
// vPoint = tri3.Get(j);
// }
// }//end for j
// if(dist < m_dEps)
// m_vPoint.push_back(vPoint);
// }
//}
//dTimeTraverse=timer0.GetTime();
//std::cout<<"Time intersection: "<<dTimeTraverse<<std::endl;
//std::cout<<"Number of intersections: "<<m_vTriangles.size()<<std::endl;
//std::cout<<"Dist checking in traversal: "<<distchecks<<std::endl;
//std::cout<<"Number of Dist checking in traversal: "<<ndistchecks<<std::endl;
//std::cout<<"adding: "<<adding<<std::endl;
return T(0);
}