C++ AABB::Set方法代码示例

// This function creates a KD tree with the given
// points, array, and length
int KDTree::create(float *setpoints, int setnpoints, int setndim,
									 bool setCopy, struct KDTreeNode *setNodeMem)
{
  ndim = setndim;
  npoints = setnpoints;
  typedef int *intptr;
	
  // Copy the points from the original array, if necessary
  copyPoints = setCopy;
  if (copyPoints) {
    if(points) delete[] points;
    points = new float[ndim*npoints];
    memcpy(points,setpoints,sizeof(float)*ndim*npoints);
  }
  // If we are not copying, just set the pointer
  else
    points = setpoints;


  // Allocate some arrays;
  if (workArr)
    delete[]workArr;
  workArr = new int[npoints];

	if(!setNodeMem) {
		if(m_Root) delete[] m_Root;
		m_Root = new struct KDTreeNode[npoints*2+1];
		nodeMemAlloc = true;
	}
	else {
		m_Root = setNodeMem;
		nodeMemAlloc = false;
	}
	nodeMemCnt = 0;

	// Alocate array used for indexing
	if(intArrMem) delete[] intArrMem;
	intArrMem = 
		new int[(int)((float)(npoints+4)*
									ceil(log((double)npoints)/log(2.0)))];
	intArrMemCnt = 0;

  // Create the "sortidx" array by 
  // sorting the range tree points on each dimension
  int **sortidx = new intptr[ndim];
	if(verbosity>1)
		logmsg("KDTree: Sorting points\n");
	float imin[3];
	float imax[3];
	imin[0] = imin[1] = imin[2] = 999999.f;
	imax[0] = imax[1] = imax[2] = -999999.f;
  for (int i = 0; i < ndim; i++) {
    // Initialize the sortidx array for this
    // dimension
    sortidx[i] = new int[npoints];

    // Initialize the "tmp" array for the sort
    int *tmp = new int[npoints];
    for (int j = 0; j < npoints; j++)
      tmp[j] = j;

    // Sort the points on dimension i, putting
    // indexes in array "tmp"
    heapsort(i,tmp,npoints);

    // sortidx is actually the inverse of the 
    // index sorts
    for (int j = 0; j < npoints; j++)
	{
		sortidx[i][tmp[j]] = j;
		imin[i] = min( points[ j*3 + i ], imin[ i ] );
		imax[i] = max( points[ j*3 + i ], imax[ i ] );
	}
    delete[] tmp;
  }
	if(verbosity > 1)
		logmsg("KDTree: Done sorting points\n");

  // Create an initial list of points that references 
  // all the points
  int *pidx = new int[npoints];
  for (int i = 0; i < npoints; i++)
    pidx[i] = i;

  // Build a KD Tree
  AABB extents;
  Vec3 vmin(  imin[0] ,
			 imin[1],
			 imin[2]  );
  Vec3 vmax(  imax[0] ,
			 imax[1],
			 imax[2]  );
  OutputVector( vmin, "VMin");
  OutputVector( vmax, "VMax");
  extents.Set( vmin,vmax );
  m_Root->bounds.Set( vmin, vmax );
	//add objects to this node
	if( m_NodeCreationCallBack )
//.........这里部分代码省略.........

// -----------------------------------------------------------
// Find nearest ray 2
// -----------------------------------------------------------
int KDTree::FindNearestToRay2( Ray& vRay, Vec3 &tmin, Vec3 &tmax, RayIntersectCallback callback, Matrix4x4 * temp )
{
	static bool bOutput = false;
	float tnear = 0;
	float tfar = ( tmax - tmin ).Length();
	int retval = 0;
	float D[3];
	float O[3];
	VecToFloatArray( vRay.m_Direction, D );
	VecToFloatArray( vRay.m_Origin, O );
	// We assume ray fits in extents
	// clip ray segment to box
	static AABB bbox;
	static int aa = 0;
	bbox.Set( tmin, tmax );
	if( !bbox.IntersectRay( vRay, tnear, tfar ) )
		return 0;
	if( tnear < 0 || 
		tfar < 0 || 
		tnear > tfar )
		return 0;
	stack< RayFindStruct > nodeStack;
	vector< KDTreeNode * > nodesToCheck;
	g_Boxes.clear();
	nodeStack.push( RayFindStruct( m_Root, tnear, tfar ) );
	while( nodeStack.size() > 0 )
	{
		RayFindStruct curSearch = nodeStack.top();
		nodeStack.pop();
		DrawBox( curSearch.node->bounds.m_Bounds[0], curSearch.node->bounds.m_Bounds[1], temp );
		
		while( curSearch.node->pntidx < 0 )//is not a leaf
		{
			int dim = curSearch.node->axis;
			float tSplit = (curSearch.node->key - O[dim]) / D[dim];
			KDTreeNode * first = m_Root + curSearch.node->leftIdx;
			KDTreeNode * second = m_Root + curSearch.node->rightIdx;
			//check dimension
			if( D[dim] < 0 )
			{
				//swap
				KDTreeNode * temp;
				temp = first;
				first = second;
				second = temp;
			}
			if( tSplit >= curSearch.tmax 
				//|| tSplit < 0 
				)
			{
				curSearch.node = first;
			}
			else 
				if( tSplit <= curSearch.tmin )
			{
				curSearch.node = second;
			}
			else
			{
				nodeStack.push( RayFindStruct( second, tSplit, curSearch.tmax ) );
				curSearch.node = first;
				curSearch.tmax = tSplit;
			}
			DrawBox( curSearch.node->bounds.m_Bounds[0], curSearch.node->bounds.m_Bounds[1], temp );
		}
		assert( curSearch.node != NULL );
		//check triangles
		nodesToCheck.push_back( curSearch.node );
		bool bresult = (*callback)( nodesToCheck );
		if( bresult )
		{
			return 1;
		}
		nodesToCheck.clear();
	}
	return 0;
	if( tnear == tfar )
	{
		return 0;
	}
	nodesToCheck.clear();
	nodesToCheck.push_back( m_Root );
	bool bresult = (*callback)( nodesToCheck );
	if( bresult )
	{
		if( g_Boxes.size() > 0 && aa < 5 
			&&
			vRay.m_Origin.z > 1000 )
		{
			OutputDebugString("RAYDEBUG START----------------------\n");
			for( int i = 0; i < (int)g_Boxes.size(); i++ )
			{
				static DWORD msgHash_AddAxisAlignedBox = CHashString(_T("AddAxisAlignedBox")).GetUniqueID();
				EngineGetToolBox()->SendMessage(msgHash_AddAxisAlignedBox,sizeof(g_Boxes[i]), &g_Boxes[i] );
				OutputDebugString("Box:\t");
				OutputVector(	g_Boxes[i].min, "Min" );
				OutputVector(	g_Boxes[i].max, "Max" );
//.........这里部分代码省略.........

// -----------------------------------------------------------
// Engine::FindNearest
// Finds the nearest intersection in a KdTree for a ray
// -----------------------------------------------------------
int KDTree::FindNearestToRay( Ray& vRay, Vec3 &tmin, Vec3 &tmax, RayIntersectCallback callback, Matrix4x4 * temp )
{
	static bool bOutput = false;
	float tnear = 0;
	float tfar = ( tmax - tmin ).Length();
	float t;
	int retval = 0;
	float D[3];
	float O[3];
	VecToFloatArray( vRay.m_Direction, D );
	VecToFloatArray( vRay.m_Origin, O );
	// We assume ray fits in extents
	// clip ray segment to box
	static AABB bbox;
	bbox.Set( tmin, tmax );
	if( !bbox.IntersectRay( vRay, tnear, tfar ) )
		return 0;
	if( tnear < 0 || 
		tfar < 0 || 
		tnear > tfar )
		return 0;
	// init stack
	int entrypoint = 0, exitpoint = 1;
	// init traversal
	KDTreeNode* farchild, *currnode;
	farchild = 0;
	currnode = m_Root;
	m_Stack[entrypoint].t = tnear;
	if (tnear > 0.0f) 
	{
		m_Stack[entrypoint].pb[0] = O[0] + D[0]*tnear;
		m_Stack[entrypoint].pb[1] = O[1] + D[1]*tnear;
		m_Stack[entrypoint].pb[2] = O[2] + D[2]*tnear;
	}
	else
	{
		m_Stack[entrypoint].pb[0] = O[0];
		m_Stack[entrypoint].pb[1] = O[1];
		m_Stack[entrypoint].pb[2] = O[2];
	}
	m_Stack[exitpoint].t = tfar;
	m_Stack[exitpoint].pb[0] = O[0] + D[0]*tfar;
	m_Stack[exitpoint].pb[1] = O[1] + D[1]*tfar;
	m_Stack[exitpoint].pb[2]= O[2] + D[2]*tfar;
	m_Stack[exitpoint].node = 0;
	int dim = -1;
	// traverse kd-tree
	char buf[1024];

	static int aa = 0;
	if( bOutput )
	{
		sprintf( buf, "tfar: %f, tnear: %f\n", tfar, tnear );
		OutputDebugString( buf );
		OutputVector( vRay.m_Origin, "Ray origin" );
		OutputVector( vRay.m_Direction, "Ray direction" );
		OutputVector( O, "Ray origin2" );
		OutputVector( D, "Ray direction2" );
		OutputVector( m_Stack[ entrypoint ].pb, "entry" );
		sprintf( buf, "exit far: %f\n", m_Stack[exitpoint].t );
		OutputDebugString( buf );
		OutputVector( m_Stack[ exitpoint ].pb, "exit" );
	}
	KDTreeNode * lastNode = 0;
	g_Boxes.clear();
	vector< KDTreeNode * > nodesToCheck;
	while(currnode)
	{
		if( aa < 1 )
			DrawBox( currnode->bounds.m_Bounds[0], currnode->bounds.m_Bounds[1], temp );
		//Is not a leaf?
		while( currnode->pntidx < 0 )
		{		
			lastNode = currnode;
			dim = (dim + 1) % ndim;
			if( m_Stack[entrypoint].pb[dim] <= currnode->key )
			{
				if( m_Stack[exitpoint].pb[dim] <= currnode->key )
				{
					currnode = m_Root + currnode->leftIdx;
					continue;
				}
				farchild = m_Root + currnode->rightIdx; // GetRight();
				currnode = m_Root + currnode->leftIdx;
			}
			else 
			{
				if (m_Stack[exitpoint].pb[dim] > currnode->key)
				{
					currnode = m_Root + currnode->rightIdx;
					continue;
				}
				farchild = m_Root + currnode->leftIdx;
				currnode = m_Root + currnode->rightIdx; // GetRight();
			}
			
//.........这里部分代码省略.........