void polygontoPolygon(Manifold* m, RigidBody* a, RigidBody* b) {
Polygon* A = reinterpret_cast<Polygon *>(a->shape);
Polygon* B = reinterpret_cast<Polygon *>(b->shape);
m->contactCount = 0;
unsigned int faceA;
float penetrationA = findAxisLeastPenetration(&faceA, A, B);
if (penetrationA >= 0.0f)
return;
unsigned int faceB;
float penetrationB = findAxisLeastPenetration(&faceB, B, A);
if (penetrationB >= 0.0f)
return;
unsigned int referenceIndex;
bool flip;
Polygon* RefPoly;
Polygon* IncPoly;
if (BiasGreaterThan(penetrationA, penetrationB)) {
RefPoly = A;
IncPoly = B;
referenceIndex = faceA;
flip = false;
}
else {
RefPoly = B;
IncPoly = A;
referenceIndex = faceB;
flip = true;
}
vec2 incidentFace[2];
findIncidentFace(incidentFace, RefPoly, IncPoly, referenceIndex);
vec2 v1 = RefPoly->m_vertices[referenceIndex];
referenceIndex = referenceIndex + 1 == RefPoly->m_vertexCount ? 0 : referenceIndex + 1;
vec2 v2 = RefPoly->m_vertices[referenceIndex];
v1 = RefPoly->m_orientation * v1 + RefPoly->m_body->getPosition();
v2 = RefPoly->m_orientation * v2 + RefPoly->m_body->getPosition();
vec2 sidePlaneNormal = (v2 - v1);
sidePlaneNormal.normalise();
vec2 refFaceNormal(sidePlaneNormal.y, -sidePlaneNormal.x);
float refC = dot(refFaceNormal, v1);
float negSide = -dot(sidePlaneNormal, v1);
float posSide = dot(sidePlaneNormal, v2);
if (clip(-sidePlaneNormal, negSide, incidentFace) < 2)
return;
if (clip(sidePlaneNormal, posSide, incidentFace) < 2)
return;
m->normal = flip ? -refFaceNormal : refFaceNormal;
unsigned int cp = 0;
float separation = dot(refFaceNormal, incidentFace[0]) - refC;
if (separation <= 0.0f) {
m->contacts[cp] = incidentFace[0];
m->penetration = -separation;
++cp;
}
else
m->penetration = 0;
separation = dot(refFaceNormal, incidentFace[1]) - refC;
if (separation <= 0.0f) {
m->contacts[cp] = incidentFace[1];
m->penetration += -separation;
++cp;
m->penetration /= (float)cp;
}
m->contactCount = cp;
}
void gridToPolygon(phys::Collision* c, phys::RigidBody *a, phys::RigidBody *b)
{
c->addManifold();
c->getLastManifold()->contactCount = 0;
GridShape* grid = reinterpret_cast<GridShape*>(a->getShape());
phys::PolygonShape* poly = reinterpret_cast<phys::PolygonShape*>(b->getShape());
sf::Vector2f topLeft(FLT_MAX, FLT_MAX);
sf::Vector2f botRight(-FLT_MAX, -FLT_MAX);
for (unsigned int i = 0; i < poly->getVertices().size(); i++)
{
// Retrieve a face normal from A
sf::Vector2f n = poly->getNormals()[i];
sf::Vector2f nw = poly->getU() * n;
// Transform face normal into B's model space
phys::Mat2 buT = grid->getU().transpose( );
n = buT * nw;
// Retrieve vertex on face from A, transform into
// B's model space
sf::Vector2f v = poly->getVertices()[i];
v = (poly->getU() * v) + b->getPosition();
v -= a->getPosition();
v = buT * v;
poly->transformedVertices[i] = v;
poly->transformedNormals[i] = n;
if (v.x < topLeft.x)
topLeft.x = v.x;
if (v.y < topLeft.y)
topLeft.y = v.y;
if (v.x > botRight.x)
botRight.x = v.x;
if (v.y > botRight.y)
botRight.y = v.y;
}
int left = floor(topLeft.x/(PTU/TILE_SIZE))-1;
int top = floor(topLeft.y/(PTU/TILE_SIZE))-1;
int right = ceil(botRight.x/(PTU/TILE_SIZE))+1;
int bot = ceil(botRight.y/(PTU/TILE_SIZE))+1;
if (!grid->getGrid()->mWrapX)
{
left = std::max(left, 0);
right = std::min(right, grid->getGrid()->mSizeX-1);
}
top = std::max(top, 0);
bot = std::min(bot, grid->getGrid()->mSizeY-1);
// Tile size
float tileSize = PTU/TILE_SIZE;
// Find the tile with the least overlap in the collision
sf::Vector2f tileVerts[4];
int tileVertCount;
float tilePenetration = 0.f;
float polyPenetration = 0.f;
int tileFace;
int polyFace;
sf::Vector2f tileNormals[4];
sf::Vector2f usedNormals[20];
int usedNormalCount = 0;
for (int y = top; y <= bot; y++)
{
for (int _x = left; _x <= right; _x++)
{
int x = _x;
if (grid->getGrid()->mWrapX)
x = grid->getGrid()->wrapX(x);
if (!grid->getGrid()->mTiles[y][x].isSolid())
continue;
int left = x-1;
int right = x+1;
int up = y-1;
int down = y+1;
if (grid->getGrid()->mWrapX)
{
left = grid->getGrid()->wrapX(left);
right = grid->getGrid()->wrapX(right);
}
bool leftT = false;
bool rightT = false;
bool upT = false;
bool downT = false;
if (left >= 0 && grid->getGrid()->mTiles[y][left].isSolid())
leftT = true;
if (right < grid->getGrid()->mSizeX && grid->getGrid()->mTiles[y][right].isSolid())
rightT = true;
if (up >= 0 && grid->getGrid()->mTiles[up][x].isSolid())
upT = true;
if (down < grid->getGrid()->mSizeY && grid->getGrid()->mTiles[down][x].isSolid())
downT = true;
sf::Vector2f start(_x*tileSize, y*tileSize);
if (!leftT && !rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start + sf::Vector2f(tileSize/2, 0);
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = normalize(sf::Vector2f(-0.5, -1.f));
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = normalize(sf::Vector2f(0.5, -1.f));
}
else if (!leftT && rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start + sf::Vector2f(tileSize, 0);
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = normalize(sf::Vector2f(-1.f, -1.f));
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = sf::Vector2f(1.f, 0.f);
}
else if (leftT && !rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start;
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = sf::Vector2f(-1.f, 0.f);
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = normalize(sf::Vector2f(1.f, -1.f));
}
else
{
tileVertCount = 4;
tileVerts[0] = start;
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileVerts[3] = start + sf::Vector2f(tileSize, 0);
tileNormals[0] = sf::Vector2f(-1, 0);
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = sf::Vector2f(1, 0);
tileNormals[3] = sf::Vector2f(0, -1);
}
// Check for a separating axis with A's face planes
int faceA;
float penetrationA = findAxisLeastPenetration(&faceA, poly->transformedVertices.data(),
poly->transformedNormals.data(), poly->transformedVertices.size(), tileVerts, tileVertCount);
if(penetrationA >= 0.0f || phys::equal(penetrationA, 0.f))
continue;
// Check for a separating axis with B's face planes
int faceB;
float penetrationB = findAxisLeastPenetration(&faceB, tileVerts, tileNormals, tileVertCount,
poly->transformedVertices.data(), poly->transformedVertices.size());
if(penetrationB >= 0.0f || phys::equal(penetrationB, 0.f))
continue;
tilePenetration = penetrationB;
polyPenetration = penetrationA;
tileFace = faceB;
polyFace = faceA;
sf::Uint32 referenceIndex;
bool flip; // Always point from a to b
phys::RigidBody* ref;
sf::Vector2f* refVerts;
sf::Vector2f* refNormals;
int refCount;
phys::RigidBody* inc;
sf::Vector2f* incVerts;
sf::Vector2f* incNormals;
int incCount;
// Determine which shape contains reference face
if(phys::biasGreaterThan( tilePenetration, polyPenetration ))
{
ref = a;
refVerts = tileVerts;
refNormals = tileNormals;
refCount = tileVertCount;
inc = b;
incVerts = poly->transformedVertices.data();
incNormals = poly->transformedNormals.data();
incCount = poly->transformedVertices.size();
referenceIndex = tileFace;
flip = false;
}
else
{
ref = b;
refVerts = poly->transformedVertices.data();
refNormals = poly->transformedNormals.data();
refCount = poly->transformedVertices.size();
inc = a;
incVerts = tileVerts;
incNormals = tileNormals;
incCount = tileVertCount;
referenceIndex = polyFace;
flip = true;
}
bool used = false;
for (int i = 0; i < usedNormalCount; i++)
{
if (refNormals[referenceIndex] == usedNormals[i])
{
used = true;
break;
}
}
if (used)
continue;
usedNormals[usedNormalCount] = refNormals[referenceIndex];
usedNormalCount++;
// World space incident face
sf::Vector2f incidentFace[2];
findIncidentFace(incidentFace, refVerts, refNormals, refCount, incVerts, incNormals, incCount, referenceIndex);
// y
// ^ ->n ^
// +---c ------posPlane--
// x < | i |\
// +---+ c-----negPlane--
// \ v
// r
//
// r : reference face
// i : incident poly
// c : clipped point
// n : incident normal
// Setup reference face vertices
sf::Vector2f v1 = refVerts[referenceIndex];
referenceIndex = referenceIndex + 1 == refCount ? 0 : referenceIndex + 1;
sf::Vector2f v2 = refVerts[referenceIndex];
// Calculate reference face side normal in world space
sf::Vector2f sidePlaneNormal = (v2 - v1);
sidePlaneNormal = phys::normalize(sidePlaneNormal);
// Orthogonalize
sf::Vector2f refFaceNormal( sidePlaneNormal.y, -sidePlaneNormal.x );
// ax + by = c
// c is distance from origin
float refC = phys::dot( refFaceNormal, v1 );
float negSide = -phys::dot( sidePlaneNormal, v1 );
float posSide = phys::dot( sidePlaneNormal, v2 );
// Clip incident face to reference face side planes
if(clip( -sidePlaneNormal, negSide, incidentFace ) < 2)
return; // Due to floating point error, possible to not have required points
if(clip( sidePlaneNormal, posSide, incidentFace ) < 2)
return; // Due to floating point error, possible to not have required points
// Flip
c->getLastManifold()->normal = flip ? -refFaceNormal : refFaceNormal;
// Keep points behind reference face
sf::Uint32 cp = 0; // clipped points behind reference face
float separation = phys::dot( refFaceNormal, incidentFace[0] ) - refC;
if(separation <= 0.0f)
{
c->getLastManifold()->contacts[cp] = incidentFace[0];
c->getLastManifold()->penetration = -separation;
++cp;
}
else
c->getLastManifold()->penetration = 0;
separation = phys::dot( refFaceNormal, incidentFace[1] ) - refC;
if(separation <= 0.0f)
{
c->getLastManifold()->contacts[cp] = incidentFace[1];
c->getLastManifold()->penetration += -separation;
++cp;
// Average penetration
c->getLastManifold()->penetration /= (float)cp;
}
c->getLastManifold()->contactCount = cp;
c->addManifold();
c->getLastManifold()->contactCount = 0;
}
}
}