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