// 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 ) { (*m_NodeCreationCallBack)( m_Root ); } build_kdtree(sortidx, // array of sort values 0, // The current dimension pidx, npoints, extents); // The list of points // Delete the sort index for (int i = 0; i < ndim; i++) delete[]sortidx[i]; delete[] sortidx; // Delete the initial list of points delete[] pidx; // Delete the sort arrays delete[] intArrMem; // delete the work array if(workArr) { delete[] workArr; workArr = (int *)NULL; } if(verbosity > 1) logmsg("KDTree: Done creating tree\n"); return 0; } // end of create
// ----------------------------------------------------------- // 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" ); } g_Boxes.clear(); aa++; static CHashString h(_T("none")); ADDLINEPARAMS LineParam; LineParam.name = &h; LineParam.start = (*temp)*vRay.m_Origin; LineParam.end = vRay.m_Origin + vRay.m_Direction*10000; LineParam.end = (*temp)*LineParam.end; LineParam.red = 0; LineParam.blue = 0; LineParam.green = 255; static DWORD msgHash_AddLine = CHashString(_T("AddLine")).GetUniqueID(); EngineGetToolBox()->SendMessage(msgHash_AddLine,sizeof(LineParam), &LineParam ); } return 1; } return 0; }
// ----------------------------------------------------------- // 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(); } t = (currnode->key - O[dim]) / D[dim]; int tmp = exitpoint++; if (exitpoint == entrypoint) exitpoint++; m_Stack[exitpoint].prev = tmp; m_Stack[exitpoint].t = t; m_Stack[exitpoint].node = farchild; m_Stack[exitpoint].pb[dim] = currnode->key; int nextaxis = (dim + 1) % ndim; int prevaxis = (dim + 2) % ndim; m_Stack[exitpoint].pb[nextaxis] = O[nextaxis] + t * D[nextaxis]; m_Stack[exitpoint].pb[prevaxis] = O[prevaxis] + t * D[prevaxis]; } if( aa < 1 ) DrawBox( currnode->bounds.m_Bounds[0], currnode->bounds.m_Bounds[1], temp ); float dist = m_Stack[exitpoint].t; nodesToCheck.push_back( currnode ); entrypoint = exitpoint; currnode = m_Stack[exitpoint].node; exitpoint = m_Stack[entrypoint].prev; } bool bfound = (*callback)( nodesToCheck ); if( bfound ) { return 1; } // return 0; //check by comparing all nodes nodesToCheck.clear(); nodesToCheck.push_back( m_Root ); bool bresult = (*callback)( nodesToCheck ); if( bresult ) { if( g_Boxes.size() > 0 && aa < 1 ) { 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" ); } g_Boxes.clear(); aa++; static CHashString h(_T("none")); ADDLINEPARAMS LineParam; LineParam.name = &h; LineParam.start = (*temp)*vRay.m_Origin; LineParam.end = vRay.m_Origin + vRay.m_Direction*10000; LineParam.end = (*temp)*LineParam.end; LineParam.red = 0; LineParam.blue = 0; LineParam.green = 255; static DWORD msgHash_AddLine = CHashString(_T("AddLine")).GetUniqueID(); EngineGetToolBox()->SendMessage(msgHash_AddLine,sizeof(LineParam), &LineParam ); } return 1; } return 0; }