private float GetEdgeDistance(PConvexPolygonShape p1,
PConvexPolygonShape p2, int edge)
{
Vector2f normal = p1.nors[edge];
Vector2f[] p1vers = p1.vers;
Vector2f[] p2vers = p2.vers;
int num = -1;
float dist = 1.0F;
for (int i = 0; i < p2.numVertices; i++)
{
float dot = normal.x * (p2vers[i].x - p2._pos.x) + normal.y
* (p2vers[i].y - p2._pos.y);
if (dist == 1.0F || dot < dist)
{
dist = dot;
num = i;
}
}
dist = normal.x * (p2vers[num].x - p1vers[edge].x) + normal.y
* (p2vers[num].y - p1vers[edge].y);
return(dist);
}
private PPolygonPolygonCollider.PWContactedVertex [] GetEdgeOfPotentialCollision(
PConvexPolygonShape p1, PConvexPolygonShape p2, int r1edge,
bool flip)
{
PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2];
Vector2f normal = p1.nors[r1edge];
float dist = 1.0F;
int ver = -1;
int nextVer = -1;
for (int i = 0; i < p2.numVertices; i++)
{
float dot = normal.Dot(p2.nors[i]);
if (dot < dist || dist == 1.0F)
{
dist = dot;
ver = i;
nextVer = (i + 1) % p2.numVertices;
}
}
line[0] = new PPolygonPolygonCollider.PWContactedVertex();
line[0].v.Set(p2.vers[ver].x, p2.vers[ver].y);
line[0].data.Set(r1edge + ver * 2 + ver * 4, false);
line[1] = new PPolygonPolygonCollider.PWContactedVertex();
line[1].v.Set(p2.vers[nextVer].x, p2.vers[nextVer].y);
line[1].data.Set(r1edge + ver * 2 + nextVer * 4, false);
return(line);
}
public PConcavePolygonShape(float[] xvers, float[] yvers, float density) {
fig = new PFigure();
tri = new PTriangulator();
poly = new PPolygonizer();
numVertices = xvers.Length;
localVers = new Vector2f[numVertices];
vers = new Vector2f[numVertices];
_dens = density;
for (int i = 0; i < numVertices; i++) {
localVers[i] = new Vector2f(xvers[i], yvers[i]);
vers[i] = new Vector2f(xvers[i], yvers[i]);
}
fig.Figure(localVers, numVertices);
numVertices = fig.numVertices;
localVers = new Vector2f[numVertices];
vers = new Vector2f[numVertices];
for (int i_0 = 0; i_0 < numVertices; i_0++) {
localVers[i_0] = new Vector2f(fig.done[i_0].x, fig.done[i_0].y);
vers[i_0] = new Vector2f(fig.done[i_0].x, fig.done[i_0].y);
}
tri.Triangulate(fig.done, fig.numVertices);
poly.Polygonize(tri.triangles, tri.numTriangles);
convexes = new PConvexPolygonShape[1024];
for (int i_1 = 0; i_1 < poly.numPolygons; i_1++) {
convexes[i_1] = new PConvexPolygonShape(poly.polygons[i_1].xs,
poly.polygons[i_1].ys, _dens);
}
numConvexes = poly.numPolygons;
CalcMassData();
_type = PShapeType.CONCAVE_SHAPE;
}
internal override void CalcAABB()
{
for (int i = 0; i < numConvexes; i++)
{
PConvexPolygonShape c = convexes[i];
c.CalcAABB();
c._sapAABB.Update();
if (i == 0)
{
_aabb.Set(c._aabb.minX, c._aabb.minY, c._aabb.maxX,
c._aabb.maxY);
}
else
{
_aabb.Set(MathUtils.Min(_aabb.minX, c._aabb.minX),
MathUtils.Min(_aabb.minY, c._aabb.minY),
MathUtils.Max(_aabb.maxX, c._aabb.maxX),
MathUtils.Max(_aabb.maxY, c._aabb.maxY));
}
}
}
internal override void Update()
{
float twoPI = MathUtils.TWO_PI;
for (int i = 0; i < numConvexes; i++)
{
PConvexPolygonShape c = convexes[i];
c._pos.Set(c._localPos.x, c._localPos.y);
_mAng.MulEqual(c._pos);
c._pos.AddLocal(_pos);
c._localAng = (c._localAng + twoPI) % twoPI;
c._ang = _ang + c._localAng;
c._mAng.SetRotate(c._ang);
c.Update();
}
for (int i_0 = 0; i_0 < numVertices; i_0++)
{
vers[i_0].Set(localVers[i_0].x, localVers[i_0].y);
_mAng.MulEqual(vers[i_0]);
vers[i_0].AddLocal(_pos);
}
}
public PConcavePolygonShape(float[] xvers, float[] yvers, float density)
{
fig = new PFigure();
tri = new PTriangulator();
poly = new PPolygonizer();
numVertices = xvers.Length;
localVers = new Vector2f[numVertices];
vers = new Vector2f[numVertices];
_dens = density;
for (int i = 0; i < numVertices; i++)
{
localVers[i] = new Vector2f(xvers[i], yvers[i]);
vers[i] = new Vector2f(xvers[i], yvers[i]);
}
fig.Figure(localVers, numVertices);
numVertices = fig.numVertices;
localVers = new Vector2f[numVertices];
vers = new Vector2f[numVertices];
for (int i_0 = 0; i_0 < numVertices; i_0++)
{
localVers[i_0] = new Vector2f(fig.done[i_0].x, fig.done[i_0].y);
vers[i_0] = new Vector2f(fig.done[i_0].x, fig.done[i_0].y);
}
tri.Triangulate(fig.done, fig.numVertices);
poly.Polygonize(tri.triangles, tri.numTriangles);
convexes = new PConvexPolygonShape[1024];
for (int i_1 = 0; i_1 < poly.numPolygons; i_1++)
{
convexes[i_1] = new PConvexPolygonShape(poly.polygons[i_1].xs,
poly.polygons[i_1].ys, _dens);
}
numConvexes = poly.numPolygons;
CalcMassData();
_type = PShapeType.CONCAVE_SHAPE;
}
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);
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != PShapeType.CIRCLE_SHAPE ||
s2._type != PShapeType.CONVEX_SHAPE &&
s2._type != PShapeType.BOX_SHAPE)
{
return(0);
}
PCircleShape c1 = (PCircleShape)s1;
PConvexPolygonShape p1 = (PConvexPolygonShape)s2;
float distance = -1F;
int edgeNumber = -1;
Vector2f[] vers = p1.vers;
int numVers = p1.numVertices;
Vector2f normal = new Vector2f();
Vector2f edgeNormal = new Vector2f();
Vector2f a = new Vector2f();
Vector2f b = new Vector2f();
int num = 0;
for (int i = 0; i < numVers; i++)
{
a.Set(c1._pos.x - vers[i].x, c1._pos.y - vers[i].y);
distance = a.Length();
distance -= c1.rad;
if (distance <= 0.0F)
{
PContact c = new PContact();
c.overlap = distance;
a.Normalize();
c.normal.Set(a.x, a.y);
c.pos.Set(vers[i].x, vers[i].y);
cs[num] = c;
if (++num == 2)
{
return(num);
}
}
}
if (num > 0)
{
return(num);
}
for (int i_0 = 0; i_0 < numVers; i_0++)
{
Vector2f ver = vers[i_0];
Vector2f nextVer = vers[(i_0 + 1) % numVers];
float edgeX = nextVer.x - ver.x;
float edgeY = nextVer.y - ver.y;
edgeNormal.Set(edgeY, -edgeX);
edgeNormal.Normalize();
a.Set(c1._pos.x - ver.x, c1._pos.y - ver.y);
b.Set(c1._pos.x - nextVer.x, c1._pos.y - nextVer.y);
if ((a.x * edgeX + a.y * edgeY) * (b.x * edgeX + b.y * edgeY) <= 0.0F)
{
float edgeLen = (float)System.Math.Sqrt(edgeX * edgeX + edgeY * edgeY);
float distanceToEdge = System.Math.Abs(a.x * edgeY - a.y * edgeX)
/ edgeLen;
if (distanceToEdge <= c1.rad)
{
distanceToEdge -= c1.rad;
if (distance > distanceToEdge || distance == -1F)
{
edgeNumber = i_0;
distance = distanceToEdge;
normal.Set(edgeNormal.x, edgeNormal.y);
}
}
}
}
if (edgeNumber > -1)
{
PContact c_1 = new PContact();
c_1.overlap = distance;
c_1.normal = normal;
c_1.pos = c1._pos.Sub(normal.Mul(c1.rad));
cs[0] = c_1;
return(1);
}
bool hit = true;
for (int i_2 = 0; i_2 < numVers; i_2++)
{
Vector2f ver = vers[i_2];
Vector2f nextVer = vers[(i_2 + 1) % numVers];
float v1x = nextVer.x - ver.x;
float v1y = nextVer.y - ver.y;
float v2x = c1._pos.x - ver.x;
float v2y = c1._pos.y - ver.y;
if (v1x * v2y - v1y * v2x >= 0.0F)
{
continue;
}
hit = false;
break;
}
if (hit)
{
distance = 1.0F;
normal = new Vector2f();
for (int i = 0; i < numVers; i++)
{
Vector2f ver = vers[i];
Vector2f nextVer = vers[(i + 1) % numVers];
a.Set(nextVer.x - ver.x, nextVer.y - ver.y);
a.Normalize();
float d = c1._pos.Sub(ver).Cross(a);
if (d < 0.0F && (distance == 1.0F || distance < d))
{
distance = d;
normal.Set(a.y, -a.x);
}
}
if (distance != 1.0F)
{
PContact c = new PContact();
c.normal.Set(normal.x, normal.y);
c.pos.Set(c1._pos.x, c1._pos.y);
c.overlap = distance;
cs[0] = c;
return(1);
}
}
return(0);
}
private PPolygonPolygonCollider.PWContactedVertex [] GetEdgeOfPotentialCollision(
PConvexPolygonShape p1, PConvexPolygonShape p2, int r1edge,
bool flip) {
PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2];
Vector2f normal = p1.nors[r1edge];
float dist = 1.0F;
int ver = -1;
int nextVer = -1;
for (int i = 0; i < p2.numVertices; i++) {
float dot = normal.Dot(p2.nors[i]);
if (dot < dist || dist == 1.0F) {
dist = dot;
ver = i;
nextVer = (i + 1) % p2.numVertices;
}
}
line[0] = new PPolygonPolygonCollider.PWContactedVertex ();
line[0].v.Set(p2.vers[ver].x, p2.vers[ver].y);
line[0].data.Set(r1edge + ver * 2 + ver * 4, false);
line[1] = new PPolygonPolygonCollider.PWContactedVertex ();
line[1].v.Set(p2.vers[nextVer].x, p2.vers[nextVer].y);
line[1].data.Set(r1edge + ver * 2 + nextVer * 4, false);
return line;
}
private float GetEdgeDistance(PConvexPolygonShape p1,
PConvexPolygonShape p2, int edge) {
Vector2f normal = p1.nors[edge];
Vector2f[] p1vers = p1.vers;
Vector2f[] p2vers = p2.vers;
int num = -1;
float dist = 1.0F;
for (int i = 0; i < p2.numVertices; i++) {
float dot = normal.x * (p2vers[i].x - p2._pos.x) + normal.y
* (p2vers[i].y - p2._pos.y);
if (dist == 1.0F || dot < dist) {
dist = dot;
num = i;
}
}
dist = normal.x * (p2vers[num].x - p1vers[edge].x) + normal.y
* (p2vers[num].y - p1vers[edge].y);
return dist;
}
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);
}
