// Compute contact points for edge versus circle. // This accounts for edge connectivity. public virtual void collideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB) { manifold.pointCount = 0; // Compute circle in frame of edge // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p)); Transform.mulToOutUnsafe(xfB, circleB.m_p, temp); Transform.mulTransToOutUnsafe(xfA, temp, Q); Vec2 A = edgeA.m_vertex1; Vec2 B = edgeA.m_vertex2; e.set_Renamed(B).subLocal(A); // Barycentric coordinates float u = Vec2.dot(e, temp.set_Renamed(B).subLocal(Q)); float v = Vec2.dot(e, temp.set_Renamed(Q).subLocal(A)); float radius = edgeA.m_radius + circleB.m_radius; // ContactFeature cf; cf.indexB = 0; cf.typeB = (sbyte)ContactID.Type.VERTEX; // Region A if (v <= 0.0f) { Vec2 _P = A; d.set_Renamed(Q).subLocal(_P); float dd = Vec2.dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.m_hasVertex0) { Vec2 A1 = edgeA.m_vertex0; Vec2 B1 = A; e1.set_Renamed(B1).subLocal(A1); float u1 = Vec2.dot(e1, temp.set_Renamed(B1).subLocal(Q)); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.indexA = 0; cf.typeA = (sbyte)ContactID.Type.VERTEX; manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.CIRCLES; manifold.localNormal.setZero(); manifold.localPoint.set_Renamed(_P); // manifold.points[0].id.key = 0; manifold.points[0].id.set_Renamed(cf); manifold.points[0].localPoint.set_Renamed(circleB.m_p); return; } // Region B if (u <= 0.0f) { Vec2 _P = B; d.set_Renamed(Q).subLocal(_P); float dd = Vec2.dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.m_hasVertex3) { Vec2 B2 = edgeA.m_vertex3; Vec2 A2 = B; Vec2 e2 = e1; e2.set_Renamed(B2).subLocal(A2); float v2 = Vec2.dot(e2, temp.set_Renamed(Q).subLocal(A2)); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.indexA = 1; cf.typeA = (sbyte)ContactID.Type.VERTEX; manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.CIRCLES; manifold.localNormal.setZero(); manifold.localPoint.set_Renamed(_P); // manifold.points[0].id.key = 0; manifold.points[0].id.set_Renamed(cf); manifold.points[0].localPoint.set_Renamed(circleB.m_p); return; } // Region AB float den = Vec2.dot(e, e); Debug.Assert(den > 0.0f); // Vec2 P = (1.0f / den) * (u * A + v * B); P.set_Renamed(A).mulLocal(u).addLocal(temp.set_Renamed(B).mulLocal(v)); P.mulLocal(1.0f / den); d.set_Renamed(Q).subLocal(P); float dd2 = Vec2.dot(d, d); if (dd2 > radius * radius) { return; } n.x = -e.y; n.y = e.x; if (Vec2.dot(n, temp.set_Renamed(Q).subLocal(A)) < 0.0f) { n.set_Renamed(-n.x, -n.y); } n.normalize(); cf.indexA = 0; cf.typeA = (sbyte)ContactID.Type.FACE; manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.FACE_A; manifold.localNormal.set_Renamed(n); manifold.localPoint.set_Renamed(A); // manifold.points[0].id.key = 0; manifold.points[0].id.set_Renamed(cf); manifold.points[0].localPoint.set_Renamed(circleB.m_p); }
/// <summary> /// Compute the collision manifold between a polygon and a circle. /// </summary> /// <param name="manifold"></param> /// <param name="polygon"></param> /// <param name="xfA"></param> /// <param name="circle"></param> /// <param name="xfB"></param> public void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB) { manifold.pointCount = 0; //Vec2 v = circle.m_p; // Compute circle position in the frame of the polygon. // before inline: Transform.mulToOut(xfB, circle.m_p, c); Transform.mulTransToOut(xfA, c, cLocal); float cLocalx = cLocal.x; float cLocaly = cLocal.y; // after inline: // final float cy = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; // final float cx = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; // final float v1x = cx - xfA.p.x; // final float v1y = cy - xfA.p.y; // final Vec2 b = xfA.q.ex; // final Vec2 b1 = xfA.q.ey; // final float cLocaly = v1x * b1.x + v1y * b1.y; // final float cLocalx = v1x * b.x + v1y * b.y; // end inline // Find the min separating edge. int normalIndex = 0; //UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'" float separation = Single.Epsilon; float radius = polygon.m_radius + circle.m_radius; int vertexCount = polygon.m_count; Vec2[] vertices = polygon.m_vertices; Vec2[] normals = polygon.m_normals; for (int i = 0; i < vertexCount; i++) { // before inline // temp.set(cLocal).subLocal(vertices[i]); // float s = Vec2.dot(normals[i], temp); // after inline Vec2 vertex = vertices[i]; float tempx = cLocalx - vertex.x; float tempy = cLocaly - vertex.y; Vec2 normal = normals[i]; float s = normal.x * tempx + normal.y * tempy; if (s > radius) { // early out return; } if (s > separation) { separation = s; normalIndex = i; } } // Vertices that subtend the incident face. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; Vec2 v1 = vertices[vertIndex1]; Vec2 v2 = vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.EPSILON) { manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.FACE_A; // before inline: // manifold.localNormal.set(normals[normalIndex]); // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f); // manifold.points[0].localPoint.set(circle.m_p); // after inline: Vec2 normal = normals[normalIndex]; manifold.localNormal.x = normal.x; manifold.localNormal.y = normal.y; manifold.localPoint.x = (v1.x + v2.x) * .5f; manifold.localPoint.y = (v1.y + v2.y) * .5f; ManifoldPoint mpoint = manifold.points[0]; mpoint.localPoint.x = circle.m_p.x; mpoint.localPoint.y = circle.m_p.y; mpoint.id.zero(); // end inline return; } // Compute barycentric coordinates // before inline: // temp.set(cLocal).subLocal(v1); // temp2.set(v2).subLocal(v1); // float u1 = Vec2.dot(temp, temp2); // temp.set(cLocal).subLocal(v2); // temp2.set(v1).subLocal(v2); // float u2 = Vec2.dot(temp, temp2); // after inline: float tempX = cLocalx - v1.x; float tempY = cLocaly - v1.y; float temp2X = v2.x - v1.x; float temp2Y = v2.y - v1.y; float u1 = tempX * temp2X + tempY * temp2Y; float temp3X = cLocalx - v2.x; float temp3Y = cLocaly - v2.y; float temp4X = v1.x - v2.x; float temp4Y = v1.y - v2.y; float u2 = temp3X * temp4X + temp3Y * temp4Y; // end inline if (u1 <= 0f) { // inlined float dx = cLocalx - v1.x; float dy = cLocaly - v1.y; if (dx * dx + dy * dy > radius * radius) { return; } manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.FACE_A; // before inline: // manifold.localNormal.set(cLocal).subLocal(v1); // after inline: manifold.localNormal.x = cLocalx - v1.x; manifold.localNormal.y = cLocaly - v1.y; // end inline manifold.localNormal.normalize(); manifold.localPoint.set_Renamed(v1); manifold.points[0].localPoint.set_Renamed(circle.m_p); manifold.points[0].id.zero(); } else if (u2 <= 0.0f) { // inlined float dx = cLocalx - v2.x; float dy = cLocaly - v2.y; if (dx * dx + dy * dy > radius * radius) { return; } manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.FACE_A; // before inline: // manifold.localNormal.set(cLocal).subLocal(v2); // after inline: manifold.localNormal.x = cLocalx - v2.x; manifold.localNormal.y = cLocaly - v2.y; // end inline manifold.localNormal.normalize(); manifold.localPoint.set_Renamed(v2); manifold.points[0].localPoint.set_Renamed(circle.m_p); manifold.points[0].id.zero(); } else { // Vec2 faceCenter = 0.5f * (v1 + v2); // (temp is faceCenter) // before inline: // temp.set(v1).addLocal(v2).mulLocal(.5f); // // temp2.set(cLocal).subLocal(temp); // separation = Vec2.dot(temp2, normals[vertIndex1]); // if (separation > radius) { // return; // } // after inline: float fcx = (v1.x + v2.x) * .5f; float fcy = (v1.y + v2.y) * .5f; float tx = cLocalx - fcx; float ty = cLocaly - fcy; Vec2 normal = normals[vertIndex1]; separation = tx * normal.x + ty * normal.y; if (separation > radius) { return; } // end inline manifold.pointCount = 1; manifold.type = Manifold.ManifoldType.FACE_A; manifold.localNormal.set_Renamed(normals[vertIndex1]); manifold.localPoint.x = fcx; // (faceCenter) manifold.localPoint.y = fcy; manifold.points[0].localPoint.set_Renamed(circle.m_p); manifold.points[0].id.zero(); } }
/// <summary> /// Compute the collision manifold between two circles. /// </summary> /// <param name="manifold"></param> /// <param name="circle1"></param> /// <param name="xfA"></param> /// <param name="circle2"></param> /// <param name="xfB"></param> public void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB) { manifold.pointCount = 0; // before inline: Transform.mulToOut(xfA, circle1.m_p, pA); Transform.mulToOut(xfB, circle2.m_p, pB); d.set_Renamed(pB).subLocal(pA); float distSqr = d.x * d.x + d.y * d.y; // after inline: // final Vec2 v = circle1.m_p; // final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y; // final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y; // // final Vec2 v1 = circle2.m_p; // final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y; // final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y; // // final float dx = pBx - pAx; // final float dy = pBy - pAy; // // final float distSqr = dx * dx + dy * dy; // end inline float radius = circle1.m_radius + circle2.m_radius; if (distSqr > radius * radius) { return; } manifold.type = Manifold.ManifoldType.CIRCLES; manifold.localPoint.set_Renamed(circle1.m_p); manifold.localNormal.setZero(); manifold.pointCount = 1; manifold.points[0].localPoint.set_Renamed(circle2.m_p); manifold.points[0].id.zero(); }
public override Shape Clone() { CircleShape shape = new CircleShape(); shape.m_p.set_Renamed(m_p); shape.m_radius = m_radius; return shape; }
public override Shape Clone() { CircleShape shape = new CircleShape(); shape.P.Set(P); shape.Radius = Radius; return shape; }
// Compute contact points for edge versus circle. // This accounts for edge connectivity. public void CollideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB) { manifold.PointCount = 0; // Compute circle in frame of edge // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p)); Transform.MulToOutUnsafe(xfB, circleB.P, temp); Transform.MulTransToOutUnsafe(xfA, temp, q); Vec2 A = edgeA.Vertex1; Vec2 B = edgeA.Vertex2; e.Set(B).SubLocal(A); // Barycentric coordinates float u = Vec2.Dot(e, temp.Set(B).SubLocal(q)); float v = Vec2.Dot(e, temp.Set(q).SubLocal(A)); float radius = edgeA.Radius + circleB.Radius; // ContactFeature cf; cf.IndexB = 0; cf.TypeB = (sbyte)ContactID.Type.Vertex; // Region A if (v <= 0.0f) { Vec2 P = A; D.Set(q).SubLocal(P); float dd = Vec2.Dot(D, D); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.HasVertex0) { Vec2 A1 = edgeA.Vertex0; Vec2 B1 = A; e1.Set(B1).SubLocal(A1); float u1 = Vec2.Dot(e1, temp.Set(B1).SubLocal(q)); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.IndexA = 0; cf.TypeA = (sbyte)ContactID.Type.Vertex; manifold.PointCount = 1; manifold.Type = Manifold.ManifoldType.Circles; manifold.LocalNormal.SetZero(); manifold.LocalPoint.Set(P); // manifold.points[0].id.key = 0; manifold.Points[0].Id.Set(cf); manifold.Points[0].LocalPoint.Set(circleB.P); return; } // Region B if (u <= 0.0f) { Vec2 P = B; D.Set(q).SubLocal(P); float dd = Vec2.Dot(D, D); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.HasVertex3) { Vec2 B2 = edgeA.Vertex3; Vec2 A2 = B; Vec2 e2 = e1; e2.Set(B2).SubLocal(A2); float v2 = Vec2.Dot(e2, temp.Set(q).SubLocal(A2)); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.IndexA = 1; cf.TypeA = (sbyte)ContactID.Type.Vertex; manifold.PointCount = 1; manifold.Type = Manifold.ManifoldType.Circles; manifold.LocalNormal.SetZero(); manifold.LocalPoint.Set(P); // manifold.points[0].id.key = 0; manifold.Points[0].Id.Set(cf); manifold.Points[0].LocalPoint.Set(circleB.P); return; } // Region AB float den = Vec2.Dot(e, e); Debug.Assert(den > 0.0f); // Vec2 P = (1.0f / den) * (u * A + v * B); p.Set(A).MulLocal(u).AddLocal(temp.Set(B).MulLocal(v)); p.MulLocal(1.0f / den); D.Set(q).SubLocal(p); float dd2 = Vec2.Dot(D, D); if (dd2 > radius * radius) { return; } n.X = -e.Y; n.Y = e.X; if (Vec2.Dot(n, temp.Set(q).SubLocal(A)) < 0.0f) { n.Set(-n.X, -n.Y); } n.Normalize(); cf.IndexA = 0; cf.TypeA = (sbyte)ContactID.Type.Face; manifold.PointCount = 1; manifold.Type = Manifold.ManifoldType.FaceA; manifold.LocalNormal.Set(n); manifold.LocalPoint.Set(A); // manifold.points[0].id.key = 0; manifold.Points[0].Id.Set(cf); manifold.Points[0].LocalPoint.Set(circleB.P); }
/// <summary> /// Compute the collision manifold between two circles. /// </summary> /// <param name="manifold"></param> /// <param name="circle1"></param> /// <param name="xfA"></param> /// <param name="circle2"></param> /// <param name="xfB"></param> public void CollideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB) { manifold.PointCount = 0; // before inline: Transform.MulToOut(xfA, circle1.P, P_A); Transform.MulToOut(xfB, circle2.P, P_B); D.Set(P_B).SubLocal(P_A); float distSqr = D.X * D.X + D.Y * D.Y; // after inline: // final Vec2 v = circle1.m_p; // final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y; // final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y; // // final Vec2 v1 = circle2.m_p; // final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y; // final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y; // // final float dx = pBx - pAx; // final float dy = pBy - pAy; // // final float distSqr = dx * dx + dy * dy; // end inline float radius = circle1.Radius + circle2.Radius; if (distSqr > radius * radius) { return; } manifold.Type = Manifold.ManifoldType.Circles; manifold.LocalPoint.Set(circle1.P); manifold.LocalNormal.SetZero(); manifold.PointCount = 1; manifold.Points[0].LocalPoint.Set(circle2.P); manifold.Points[0].Id.Zero(); }