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