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
示例#2
0
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);
}