bool f2PolygontoPolygon(f2Manifold* m, f2PolygonShape* A, f2PolygonShape* B)
{
// 初始化接触点
m->touchPointsNum = 0;
// 最短深度(数值最大)的面序号
uint32 faceA;
// 通过a上的法向量寻找b上的支点
float32 penetrationA = f2FindMaxSeparation(&faceA, A, B);
if(penetrationA >= 0.0f)
// 碰撞没有发生
return false;
uint32 faceB;
float32 penetrationB = f2FindMaxSeparation(&faceB, B, A);
if(penetrationB >= 0.0f)
// 碰撞没有发生
return false;
// 碰撞检测完毕 之后生成碰撞的深度及其他信息
uint32 referenceIndex;
bool flip;
// --!
f2PolygonShape *referencePoly;
f2PolygonShape *incidentPoly;
// 决定哪个多边形包含了参照面(深度大的包含参考面) 深度较小(数值较大--小于0)的可以作为后期用来分离两个多边形
// 这里的深度一定小于0 在面法线上的投影越长越小
// A>B?提供一定容忍度
//const float32 tolerance = 0.1f * F2_LINEARSLOP;
//if(penetrationA > penetrationB + tolerance)
if(f2Greater(penetrationA, penetrationB))
{
referencePoly = A;
incidentPoly = B;
referenceIndex = faceA;
flip = false;
}
else
{
referencePoly = B;
incidentPoly = A;
referenceIndex = faceB;
flip = true;
}
// 找到碰撞发生面
f2Vector2f incidentFace[2];
f2FindIncidentFace(incidentFace, referencePoly, incidentPoly, referenceIndex);
// 参考面顶点
f2Vector2f v1 = referencePoly->getVertice(referenceIndex);
// 循环
referenceIndex = referenceIndex + 1 == referencePoly->getVertexCount() ? 0 : referenceIndex + 1;
f2Vector2f v2 = referencePoly->getVertice(referenceIndex);
// 转换到参考多边形的世界坐标系中
v1 = referencePoly->getOrient() * v1 + referencePoly->getPosition();
v2 = referencePoly->getOrient() * v2 + referencePoly->getPosition();
// 在世界坐标系中计算参照多边形的边向量
f2Vector2f sidePlaneNormal = (v2 - v1);
// 这里的normal表示后期计算时操作的是垂直于sidePlane的直线而其本身代表的还是边(v2 - v1)
sidePlaneNormal.normalize();
//得到边的法线向量(以当前向量方向为基准 顺时针旋转90度得到refNormal)
f2Vector2f refFaceNormal(sidePlaneNormal.y, -sidePlaneNormal.x);
// ax + by = c
// 联立两点式直线方程
// c = x2y1 - x1y2
// a = y2 - y1
// b = x1 - x2
// 这条直线过v1 v2点
float32 referenceC = refFaceNormal * v1;
// ->n ^
// +---c ------positivePlane--clip
// | i |\
// +---+ c-----negetivePlane--clip
// \ v
// r
//
// r : 参考面
// i : 事件多边形
// c : 裁剪点
// n : 事件面法线
// 裁剪线直线方程的c
float32 negetiveC = -(sidePlaneNormal * v1);
float32 positiveC = sidePlaneNormal * v2;
// 在相对面上裁剪
if(f2Clip(-sidePlaneNormal, negetiveC, incidentFace) < 2)
return false;
// 相向面裁剪
if(f2Clip(sidePlaneNormal, positiveC, incidentFace) < 2)
return false;
// 是否翻转参考面面法线
m->normal = flip ? -refFaceNormal : refFaceNormal;
// 保持点在参考面后方
uint32 contactNum = 0;
// 这里得到的实际方程式为ax+by+c 也就是经过v1 v2点的直线 含义为d = ax+by+c尽管这里的d 还应该除以(a^2+b^2)^1/2不过猜测应该是为了提高计算速度有关系
float32 separation = refFaceNormal * incidentFace[0] - referenceC;
if(separation <= 0.0f)
{
m->touchPoints[contactNum] = incidentFace[0];
// 深度为负值
m->penetration = -separation;
++contactNum;
}
else
m->penetration = 0;
separation = refFaceNormal * incidentFace[1] - referenceC;
if(separation <= 0.0f)
{
m->touchPoints[contactNum] = incidentFace[1];
m->penetration += -separation;
++contactNum;
// 平均两接触点的接触深度
m->penetration /= (float32) contactNum;
}
m->touchPointsNum = contactNum;
return true;
}
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;
}
}
}
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 Collider::PolygonToPolygon( Manifold *m, Body *a, Body *b )
{
PolygonShape *A=reinterpret_cast<PolygonShape *>(a->m_shape);
PolygonShape *B=reinterpret_cast<PolygonShape *>(b->m_shape);
m->m_contactCount=0;
int faceA;
float penetrationA=FindAxisLeastPenetration(&faceA,A,B);
if(penetrationA>=0.0f)
{
return;
}
int faceB;
float penetrationB=FindAxisLeastPenetration(&faceB,B,A);
if(penetrationB>=0.0f)
{
return;
}
int referenceIndex;
bool flip;
PolygonShape *RefPoly;
PolygonShape *IncPoly;
if(MPACK::Math::Misc<float>::BiasGreaterThan(penetrationA,penetrationB))
{
RefPoly = A;
IncPoly = B;
referenceIndex = faceA;
flip = false;
}
else
{
RefPoly = B;
IncPoly = A;
referenceIndex = faceB;
flip = true;
}
Vector2f incidentFace[2];
FindIncidentFace(incidentFace,RefPoly,IncPoly,referenceIndex);
Vector2f v1=RefPoly->m_vertices[referenceIndex];
referenceIndex=referenceIndex+1==RefPoly->m_vertexCount?0:referenceIndex+1;
Vector2f v2=RefPoly->m_vertices[referenceIndex];
v1 = RefPoly->u * v1 + RefPoly->body->m_position;
v2 = RefPoly->u * v2 + RefPoly->body->m_position;
Vector2f sidePlaneNormal=(v2-v1);
sidePlaneNormal.Normalize();
Vector2f 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->m_normal=flip?-refFaceNormal:refFaceNormal;
int contactPoints=0;
float separation=Dot(refFaceNormal,incidentFace[0])-refC;
if(separation<=0.0f)
{
m->m_contacts[contactPoints]=incidentFace[0];
m->m_penetration=-separation;
++contactPoints;
}
else
{
m->m_penetration=0;
}
separation=Dot(refFaceNormal,incidentFace[1])-refC;
if(separation<=0.0f)
{
m->m_contacts[contactPoints]=incidentFace[1];
m->m_penetration+=-separation;
++contactPoints;
m->m_penetration/=(float)contactPoints;
}
m->m_contactCount=contactPoints;
}
