private void CollisionShape(PShape s1, PShape s2, PCollisionChooser cc_0)
{
PContact[] cs = new PContact[2];
int num = cc_0.Collide(s1, s2, cs);
if (num == 0)
{
return;
}
bool found = false;
for (int f = 0; f < numSolvers; f++)
{
if (s1 != solvers[f].s1 || s2 != solvers[f].s2)
{
continue;
}
solvers[f].Update(cs, num);
found = true;
break;
}
if (!found)
{
PSolver solver = new PSolver(s1, s2, cs, num);
AddSolver(solver);
}
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != Physics.PShapeType.CIRCLE_SHAPE ||
s2._type != Physics.PShapeType.CIRCLE_SHAPE)
{
return(0);
}
PCircleShape c1 = (PCircleShape)s1;
PCircleShape c2 = (PCircleShape)s2;
Vector2f normal = c2._pos.Sub(c1._pos);
float rad = c1.rad + c2.rad;
float length = normal.Length();
if (length < rad)
{
PContact c = new PContact();
c.overlap = length - rad;
normal.Normalize();
c.pos.Set(c1._pos.x + normal.x * c1.rad, c1._pos.y + normal.y
* c1.rad);
c.normal.Set(-normal.x, -normal.y);
cs[0] = c;
return(1);
}
else
{
return(0);
}
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs) {
if (s1._type != Physics.PShapeType.CIRCLE_SHAPE
|| s2._type != Physics.PShapeType.CIRCLE_SHAPE) {
return 0;
}
PCircleShape c1 = (PCircleShape) s1;
PCircleShape c2 = (PCircleShape) s2;
Vector2f normal = c2._pos.Sub(c1._pos);
float rad = c1.rad + c2.rad;
float length = normal.Length();
if (length < rad) {
PContact c = new PContact();
c.overlap = length - rad;
normal.Normalize();
c.pos.Set(c1._pos.x + normal.x * c1.rad, c1._pos.y + normal.y
* c1.rad);
c.normal.Set(-normal.x, -normal.y);
cs[0] = c;
return 1;
} else {
return 0;
}
}
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);
}
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);
}
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 void CollisionShape(PShape s1, PShape s2, PCollisionChooser cc_0) {
PContact[] cs = new PContact[2];
int num = cc_0.Collide(s1, s2, cs);
if (num == 0) {
return;
}
bool found = false;
for (int f = 0; f < numSolvers; f++) {
if (s1 != solvers[f].s1 || s2 != solvers[f].s2) {
continue;
}
solvers[f].Update(cs, num);
found = true;
break;
}
if (!found) {
PSolver solver = new PSolver(s1, s2, cs, num);
AddSolver(solver);
}
}
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;
}
