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; }
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; }