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 {};
}
