private PPolygonPolygonCollider.PWDistanceData GetDistance(PConvexPolygonShape p1,
PConvexPolygonShape p2)
{
PPolygonPolygonCollider.PWDistanceData distance = new PPolygonPolygonCollider.PWDistanceData();
Vector2f firstScan = p2._pos.Sub(p1._pos);
float dist = 1.0F;
int edgeNumber = -1;
for (int i = 0; i < p1.numVertices; i++)
{
float dot = p1.nors[i].Dot(firstScan);
if (dot > dist || dist == 1.0F)
{
dist = dot;
edgeNumber = i;
}
}
float edgeDist = GetEdgeDistance(p1, p2, edgeNumber);
if (edgeDist > 0.0F)
{
distance.dist = edgeDist;
distance.edge = -1;
return(distance);
}
float nextEdgeDist = GetEdgeDistance(p1, p2, (edgeNumber + 1)
% p1.numVertices);
if (nextEdgeDist > 0.0F)
{
distance.dist = nextEdgeDist;
distance.edge = -1;
return(distance);
}
float prevEdgeDist = GetEdgeDistance(p1, p2,
((edgeNumber + p1.numVertices) - 1) % p1.numVertices);
if (prevEdgeDist > 0.0F)
{
distance.dist = prevEdgeDist;
distance.edge = -1;
return(distance);
}
float mimimumDistance;
int mimimumEdgeNumber;
if (edgeDist > nextEdgeDist && edgeDist > prevEdgeDist)
{
mimimumDistance = edgeDist;
mimimumEdgeNumber = edgeNumber;
distance.dist = mimimumDistance;
distance.edge = mimimumEdgeNumber;
return(distance);
}
int signal;
if (nextEdgeDist > prevEdgeDist)
{
mimimumDistance = nextEdgeDist;
mimimumEdgeNumber = (edgeNumber + 1) % p1.numVertices;
signal = 1;
}
else
{
mimimumDistance = prevEdgeDist;
mimimumEdgeNumber = ((edgeNumber + p1.numVertices) - 1)
% p1.numVertices;
signal = p1.numVertices - 1;
}
do
{
edgeNumber = (mimimumEdgeNumber + signal) % p1.numVertices;
nextEdgeDist = GetEdgeDistance(p1, p2, edgeNumber);
if (nextEdgeDist > 0.0F)
{
distance.dist = nextEdgeDist;
distance.edge = -1;
return(distance);
}
if (nextEdgeDist > mimimumDistance)
{
mimimumEdgeNumber = edgeNumber;
mimimumDistance = nextEdgeDist;
}
else
{
distance.dist = mimimumDistance;
distance.edge = mimimumEdgeNumber;
return(distance);
}
} while (true);
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != PShapeType.CONVEX_SHAPE &&
s1._type != PShapeType.BOX_SHAPE ||
s2._type != PShapeType.CONVEX_SHAPE &&
s2._type != PShapeType.BOX_SHAPE)
{
return(0);
}
PConvexPolygonShape p1 = (PConvexPolygonShape)s1;
PConvexPolygonShape p2 = (PConvexPolygonShape)s2;
PPolygonPolygonCollider.PWDistanceData dis1 = GetDistance(p1, p2);
if (dis1.dist > 0.0F)
{
return(0);
}
PPolygonPolygonCollider.PWDistanceData dis2 = GetDistance(p2, p1);
if (dis2.dist > 0.0F)
{
return(0);
}
float error = 0.008F;
int edgeA;
PConvexPolygonShape pa;
PConvexPolygonShape pb;
bool flip;
if (dis1.dist > dis2.dist + error)
{
pa = p1;
pb = p2;
edgeA = dis1.edge;
flip = false;
}
else
{
pa = p2;
pb = p1;
edgeA = dis2.edge;
flip = true;
}
Vector2f normal = pa.nors[edgeA];
Vector2f tangent = new Vector2f(-normal.y, normal.x);
Vector2f[] paVers = pa.vers;
PPolygonPolygonCollider.PWContactedVertex [] cv = GetEdgeOfPotentialCollision(pa, pb, edgeA,
flip);
cv = ClipEdge(cv, tangent.Negate(), -tangent.Dot(paVers[edgeA]));
if (cv == null)
{
return(0);
}
cv = ClipEdge(cv, tangent,
tangent.Dot(paVers[(edgeA + 1) % pa.numVertices]));
if (cv == null)
{
return(0);
}
Vector2f contactNormal = (flip) ? normal : normal.Negate();
int numContacts = 0;
for (int i = 0; i < 2; i++)
{
float dist = normal.Dot(cv[i].v) - normal.Dot(paVers[edgeA]);
if (dist < 0.0F)
{
PContact c = new PContact();
c.normal.Set(contactNormal.x, contactNormal.y);
c.pos.Set(cv[i].v.x, cv[i].v.y);
c.overlap = dist;
c.data = cv[i].data;
c.data.flip = flip;
cs[numContacts] = c;
numContacts++;
}
}
return(numContacts);
}
private PPolygonPolygonCollider.PWDistanceData GetDistance(PConvexPolygonShape p1,
PConvexPolygonShape p2) {
PPolygonPolygonCollider.PWDistanceData distance = new PPolygonPolygonCollider.PWDistanceData ();
Vector2f firstScan = p2._pos.Sub(p1._pos);
float dist = 1.0F;
int edgeNumber = -1;
for (int i = 0; i < p1.numVertices; i++) {
float dot = p1.nors[i].Dot(firstScan);
if (dot > dist || dist == 1.0F) {
dist = dot;
edgeNumber = i;
}
}
float edgeDist = GetEdgeDistance(p1, p2, edgeNumber);
if (edgeDist > 0.0F) {
distance.dist = edgeDist;
distance.edge = -1;
return distance;
}
float nextEdgeDist = GetEdgeDistance(p1, p2, (edgeNumber + 1)
% p1.numVertices);
if (nextEdgeDist > 0.0F) {
distance.dist = nextEdgeDist;
distance.edge = -1;
return distance;
}
float prevEdgeDist = GetEdgeDistance(p1, p2,
((edgeNumber + p1.numVertices) - 1) % p1.numVertices);
if (prevEdgeDist > 0.0F) {
distance.dist = prevEdgeDist;
distance.edge = -1;
return distance;
}
float mimimumDistance;
int mimimumEdgeNumber;
if (edgeDist > nextEdgeDist && edgeDist > prevEdgeDist) {
mimimumDistance = edgeDist;
mimimumEdgeNumber = edgeNumber;
distance.dist = mimimumDistance;
distance.edge = mimimumEdgeNumber;
return distance;
}
int signal;
if (nextEdgeDist > prevEdgeDist) {
mimimumDistance = nextEdgeDist;
mimimumEdgeNumber = (edgeNumber + 1) % p1.numVertices;
signal = 1;
} else {
mimimumDistance = prevEdgeDist;
mimimumEdgeNumber = ((edgeNumber + p1.numVertices) - 1)
% p1.numVertices;
signal = p1.numVertices - 1;
}
do {
edgeNumber = (mimimumEdgeNumber + signal) % p1.numVertices;
nextEdgeDist = GetEdgeDistance(p1, p2, edgeNumber);
if (nextEdgeDist > 0.0F) {
distance.dist = nextEdgeDist;
distance.edge = -1;
return distance;
}
if (nextEdgeDist > mimimumDistance) {
mimimumEdgeNumber = edgeNumber;
mimimumDistance = nextEdgeDist;
} else {
distance.dist = mimimumDistance;
distance.edge = mimimumEdgeNumber;
return distance;
}
} while (true);
}
