void UnCollide(GameObject* go, const Vec3f& oldPos, const AABB& aabb) { /* Move back so not interpenetrating: Boxes now intersect in all axes. Moving away by penetration depth in any axis will resolve the intersection. To choose which axis, use the previous position. E.g. if previously not intersecting in X, move away in X axis. */ // Intersecton region AABB ir = aabb.Intersection(go->GetAABB()); Vec3f goPos = go->GetPos(); AABB oldBox = go->GetAABB(); Vec3f move = oldPos - goPos; oldBox.Translate(move); // Oldbox should NOT be intersecting in one or more axes Vec3f penDist( ir.GetMax(0) - ir.GetMin(0), ir.GetMax(1) - ir.GetMin(1), ir.GetMax(2) - ir.GetMin(2)); penDist *= 1.01f; // move away a bit more to be sure of clearing the collision if (oldBox.GetMax(0) < aabb.GetMin(0)) { // Old box to left of box, so move away to the left goPos.x -= penDist.x; } else if (oldBox.GetMin(0) > aabb.GetMax(0)) { goPos.x += penDist.x; } /* else if (oldBox.GetMax(1) < aabb.GetMin(1)) { goPos.y -= penDist.y; } else if (oldBox.GetMin(1) > aabb.GetMax(1)) { goPos.y += penDist.y; } */ else if (oldBox.GetMax(2) < aabb.GetMin(2)) { goPos.z -= penDist.z; } else if (oldBox.GetMin(2) > aabb.GetMax(2)) { goPos.z += penDist.z; } go->SetPos(goPos); }
// recursive construction algorithm //void Kdtree::construct(unsigned int fn, const AABB& box, const std::list<unsigned int>& triList, AABB * bounds, unsigned int depth) //void Kdtree::construct(unsigned int fn, AABB& box, std::list<IPrimitive*>& primList, AABB * bounds, unsigned int depth) void Kdtree::construct(unsigned int fn, AABB& box, unsigned int size, unsigned int depth, bool isLeft, unsigned int rIndex, unsigned int pre_size, bool dsize, int tries) { //BSPNode *node = &m_pTree[fn]; //printf("max num: %i\n", m_pTree.max_size()); //m_pTree.assign(BSPNode(), fn); BSPNode *node = &m_pTree[fn]; //unsigned int size = (unsigned int)primList.size(); //std::list<IPrimitive*>::iterator p; //printf("."); // if end condition met // if(abs(previous size - size) <= 3 ... if((int)abs((int)(pre_size-size)) <= 3) { dsize = true; tries++; } else { dsize = false; tries = 0; } if(size <= m_MaxItemPerNode || depth >= m_MaxDepth || (dsize && tries == 3)) { IPrimitive** myList = 0; if(size > 0) myList = new IPrimitive*[size]; //std::list<IPrimitive*>* myList = new std::list<IPrimitive*>; if(depth == 0) { std::list<IPrimitive*>::iterator tit; int i = 0; for(tit = m_PrimList.begin(); tit != m_PrimList.end(); tit++, i++) myList[i] = (*tit); } else if(isLeft) { for(unsigned int i = 0; i < size; i++) myList[i] = leftBOX[i]; } else { for(unsigned int i = 0; i < size; i++) myList[i] = rightBOX[rIndex+i]; } node->initLeaf(size, myList); // increment counts m_TotalTreeTri += size; totalLeaf++; return; // leaf with triangle } // each node must have a split axis and split position int axis = 0; float splitpos = 0; unsigned int i; #ifdef SAH AABB tbox; // build up information of bounding box and edges if(depth == 0) { i = 0; std::list<IPrimitive*>::iterator tit; for(tit = m_PrimList.begin(); tit != m_PrimList.end(); tit++, i++) { tbox = (*tit)->getAABB(); AABB* mybox = &(box.Intersection(tbox)); // get intersection of triangle box + bounding box of this node Point3 minp = mybox->getPos(); Point3 maxp = minp + mybox->getSize(); // add x position to the list edge[0][i*2].set(minp.X(), (*tit), true); edge[0][i*2+1].set(maxp.X(), (*tit), false); // add y edge[1][i*2].set(minp.Y(), (*tit), true); edge[1][i*2+1].set(maxp.Y(), (*tit), false); // add z edge[2][i*2].set(minp.Z(), (*tit), true); edge[2][i*2+1].set(maxp.Z(), (*tit), false); } } else if(isLeft) { for(i = 0; i < size; i++) { tbox = leftBOX[i]->getAABB(); AABB* mybox = &(box.Intersection(tbox)); // get intersection of triangle box + bounding box of this node Point3 minp = mybox->getPos(); Point3 maxp = minp + mybox->getSize(); // add x position to the list edge[0][i*2].set(minp.X(), 0, true); edge[0][i*2+1].set(maxp.X(), 0, false); // add y edge[1][i*2].set(minp.Y(), 0, true); edge[1][i*2+1].set(maxp.Y(), 0, false); // add z edge[2][i*2].set(minp.Z(), 0, true); edge[2][i*2+1].set(maxp.Z(), 0, false); } } else { for(i = 0; i < size; i++) { unsigned int tIndex = rIndex + i; tbox = rightBOX[tIndex]->getAABB(); AABB* mybox = &(box.Intersection(tbox)); // get intersection of triangle box + bounding box of this node Point3 minp = mybox->getPos(); Point3 maxp = minp + mybox->getSize(); // add x position to the list edge[0][i*2].set(minp.X(), 0, true); edge[0][i*2+1].set(maxp.X(), 0, false); // add y edge[1][i*2].set(minp.Y(), 0, true); edge[1][i*2+1].set(maxp.Y(), 0, false); // add z edge[2][i*2].set(minp.Z(), 0, true); edge[2][i*2+1].set(maxp.Z(), 0, false); } } // calculate SAH and split position float cost = INFINITY; sahSplit(axis, splitpos, cost, size, box, edge); // figure out best split /* // old stuff if(!bounds) bounds = new AABB[size]; // create a container for all triangle bbox for( i = 0; i < 3; i++ ) { edge[i] = new boundedge[2*size]; } AABB tbox; // build up information of bounding box and edges for(p = primList.begin(), i = 0; p != primList.end(); p++, i++) { //AABB* tbox = &(*p)->getAABB(); if(depth == 0) { //unsigned int index = (*uit); //unsigned int model = getTrueIndex(index); //Triangle tt = myScene->getModel(model)->getTriangle(index); // tbox = new AABB(tt); // bounds[i] = (*tbox); //IPrimitive p; tbox = (*p)->getAABB(); bounds[i] = tbox; } else { tbox = bounds[i]; } AABB* mybox = &(box.Intersection(tbox)); // get intersection of triangle box + bounding box of this node Point3 minp = mybox->getPos(); Point3 maxp = minp + mybox->getSize(); // add x position to the list edge[0][i*2] = boundedge(minp.X(), (*p), true); edge[0][i*2+1] = boundedge(maxp.X(), (*p), false); // add y edge[1][i*2] = boundedge(minp.Y(), (*p), true); edge[1][i*2+1] = boundedge(maxp.Y(), (*p), false); // add z edge[2][i*2] = boundedge(minp.Z(), (*p), true); edge[2][i*2+1] = boundedge(maxp.Z(), (*p), false); } // calculate SAH and split position float cost = INFINITY; sahSplit(axis, splitpos, cost, size, box, edge); // figure out best split */ // no good(better) split position can be found if(cost == INFINITY) { IPrimitive** myList = 0; if(size > 0) myList = new IPrimitive*[size]; //std::list<IPrimitive*>* myList = new std::list<IPrimitive*>; if(depth == 0) { std::list<IPrimitive*>::iterator tit; int i = 0; for(tit = m_PrimList.begin(); tit != m_PrimList.end(); tit++, i++) myList[i] = (*tit); } else if(isLeft) { for(unsigned int i = 0; i < size; i++) myList[i] = leftBOX[i]; } else { for(unsigned int i = 0; i < size; i++) myList[i] = rightBOX[rIndex+i]; } node->initLeaf(size, myList); // increment counts m_TotalTreeTri += size; totalLeaf++; return; // leaf with triangle } #else axis = depth % 3; Point3 minp = box.getPos(); Point3 maxp = minp + box.getSize(); splitpos = (minp[axis] + maxp[axis])/2; #endif // a split position was calculated, // so create left and right nodes m_NodeUsed += 2; //if(m_NodeUsed == 21) // printf("what"); //m_pTree.push_back(BSPNode()); //m_pTree.push_back(BSPNode()); // if we need more node if(m_NodeUsed >= m_MaxNode/* || rSize >= m_MaxNode*3*/) { try { BSPNode *tt = new BSPNode[m_MaxNode*2]; memcpy(tt, m_pTree, m_MaxNode*sizeof(BSPNode)); free(m_pTree); m_pTree = tt; node = &m_pTree[fn]; m_MaxNode *= 2; } catch(std::bad_alloc&) { deleteTree(); printf("no memory\n"); return; } } // figure out left voxel and right voxel Point3 rminp = box.getPos(); Point3 lmaxp = box.getPos() + box.getSize(); rminp[axis] = splitpos; lmaxp[axis] = splitpos; AABB lvoxel( box.getPos(), lmaxp ); AABB rvoxel( rminp, box.getPos() + box.getSize() ); int a,b; int rcIndex; // right index pass down to child bool hit = false; a = b = 0; if(depth == 0) // special case at depth 0, triangle information are still in the list { std::list<IPrimitive*>::iterator tit; for(tit = m_PrimList.begin(); tit != m_PrimList.end(); tit++) { if( lvoxel.Intersect( (*tit)->getAABB() ) ) { leftBOX[a] = (*tit);//.push_back(&(*tit)); a++; hit = true; } if( rvoxel.Intersect( (*tit)->getAABB() ) ) { rightBOX.push_back((*tit)); //rightBOX[rSize+b] = &(*tit);//.push_back(&(*tit)); b++; hit = true; } if(!hit) { leftBOX[a] = (*tit); a++; rightBOX.push_back((*tit)); b++; } hit = false; } rcIndex = rSize; rSize += b; } else { if(isLeft) // use left BOX , so data is overwritten { for(i = 0; i < size; i++) { if( lvoxel.Intersect( leftBOX[i]->getAABB() ) ) { leftBOX[a] = leftBOX[i];//.push_back(&(*tit)); hit = true; a++; } if( rvoxel.Intersect( leftBOX[i]->getAABB() ) ) { rightBOX.push_back(leftBOX[i]); //rightBOX[rSize+b] = primList[i];//.push_back(&(*tit)); b++; hit = true; } if(!hit) { leftBOX[a] = leftBOX[i]; a++; rightBOX.push_back(leftBOX[i]); b++; } hit = false; } } else // use right BOX, so data is appended { unsigned int tSize = rIndex+size; for(i = rIndex; i < tSize; i++) { if( lvoxel.Intersect( rightBOX[i]->getAABB() ) )//primList[i]->getAABB() ) ) { leftBOX[a] = rightBOX[i];//primList[i];//.push_back(&(*tit)); a++; hit = true; } if( rvoxel.Intersect( rightBOX[i]->getAABB()/*primList[i]->getAABB()*/ ) ) { rightBOX.push_back(rightBOX[i]); //rightBOX[rSize+b] = rightBOX[i];//primList[i];//.push_back(&(*tit)); b++; hit = true; } if(!hit) { leftBOX[a] = rightBOX[i]; a++; rightBOX.push_back(rightBOX[i]); b++; } hit = false; } } rcIndex = rSize; rSize += b; } // old stuff /* // distribute primitives std::list<IPrimitive*> leftList; std::list<IPrimitive*> rightList; for(p = primList.begin(); p != primList.end(); p++) { //i/f(fn == 20) // printf("why\n"); if( lvoxel.Intersect( (*p)->getAABB() ) ) { leftList.push_back(*p); } if( rvoxel.Intersect( (*p)->getAABB() ) ) { rightList.push_back(*p); } } */ // produce branch node->initNode(axis, splitpos); node->leftChild = m_NodeUsed-2; unsigned int left = node->leftChild; construct(left, lvoxel, a, depth+1, 1, 0, size, dsize, tries); construct(left+1, rvoxel, b, depth+1, 0, rcIndex, size, dsize, tries); }