vector<pair<RigidBody *, RigidBody *>> &NSquared::ComputePairs() { ColliderPairList.clear(); //Outer Loop for(vector<RigidBody *>::iterator i = ColliderList.begin(); i != ColliderList.end(); ++i) { vector<RigidBody *>::iterator jStart = i; for(vector<RigidBody *>::iterator j = ++jStart; j != ColliderList.end(); ++j) { RigidBody *rBodyA = *i; RigidBody *rBodyB = *j; AABB *aabbA = rBodyA->GetBoundingBox(); AABB *aabbB = rBodyB->GetBoundingBox(); if(aabbA->checkCollision(*aabbB)) { ColliderPairList.push_back(make_pair(rBodyA, rBodyB)); } } } return ColliderPairList; }
Maybe<Collision> World::checkCollision(const AABB& entity) const { /* std::cout << entity.center << "\t" << entity.size << std::endl; */ Vec3i minBlock,maxBlock; for(int i=0;i<3;++i) { int blocks[] = { (int)std::ceil(entity.center[i]+entity.size[i]/2), (int)std::floor(entity.center[i]-entity.size[i]/2) }; if(blocks[0] < blocks[1]) { minBlock[i] = blocks[0]; maxBlock[i] = blocks[1]; } else { minBlock[i] = blocks[1]; maxBlock[i] = blocks[0]; } } /* std::cout << minBlock << "\t" << maxBlock << std::endl; */ std::vector<Vec3f> blockOverlaps; Vec3i regionSize = maxBlock-minBlock+Vec3i{{1,1,1}}; blockOverlaps.reserve(regionSize[0]*regionSize[1]*regionSize[2]); auto calcIndex = [minBlock,regionSize](int x,int y,int z) { return (x-minBlock[0])*(regionSize[1]*regionSize[2]) +(z-minBlock[2])*regionSize[1] +(y-minBlock[1]); }; bool collision = false; for(int x=minBlock[0];x<=maxBlock[0];++x) { for(int z=minBlock[2];z<=maxBlock[2];++z) { for(int y=minBlock[1];y<=maxBlock[1];++y) { auto b = getBlock(x,y,z); if(!b.filled) { continue; } Vec3f loc = {{x+0.5f,y+0.5f,z+0.5f}}; AABB block = { loc,{{1,1,1}} }; auto blockCollision = entity.checkCollision(block,false); Collision c; if(blockCollision.extract(c)) { auto blockOverlap = c.overlap; blockOverlaps.push_back(blockOverlap); collision = true; } else { blockOverlaps.push_back({{0,0,0}}); } } } } for(int x=minBlock[0];x<=maxBlock[0];++x) { for(int z=minBlock[2];z<=maxBlock[2];++z) { for(int y=minBlock[1];y<=maxBlock[1];++y) { Vec3i loc = {{x,y,z}}; auto& overlap = blockOverlaps[calcIndex(x,y,z)]; for(int i=0;i<3;++i) { int dir = signum(overlap[i]); auto otherLoc = loc; otherLoc[i] += dir; if(otherLoc[i] < minBlock[i] || otherLoc[i] > maxBlock[i]) { continue; } auto& otherOverlap = blockOverlaps[calcIndex(otherLoc[0], otherLoc[1], otherLoc[2])]; if(dir != 0 && signum(otherOverlap[i]) == -dir) { otherOverlap[i] = 0; AABB combined; combined.center = loc+Vec3f{{0.5,0.5,0.5}}; combined.center[i] = (dir > 0 ? otherLoc[i] : loc[i]); combined.size = {{1,1,1}}; combined.size[i] = 2; Collision collision; entity.checkCollision(combined).extract(collision); overlap[i] = collision.overlap[i]; } } } } } Vec3f totalOverlap; for(auto overlap:blockOverlaps) { totalOverlap += overlap; } int minAxis = 0; for(int i=1;i<3;++i) { if(std::abs(totalOverlap[i]) < std::abs(totalOverlap[minAxis])) { minAxis = i; } } totalOverlap[(minAxis+1)%3] = totalOverlap[(minAxis+2)%3] = 0; if(collision) { return {{totalOverlap}}; } return {}; }