protected internal Contact(IWorldPool argPool) { FixtureA = null; FixtureB = null; NodeA = new ContactEdge(); NodeB = new ContactEdge(); Manifold = new Manifold(); Pool = argPool; }
public virtual void collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB) { Transform.mulTransToOutUnsafe(xfA, xfB, m_xf); Transform.mulToOutUnsafe(m_xf, polygonB.m_centroid, m_centroidB); m_v0 = edgeA.m_vertex0; m_v1 = edgeA.m_vertex1; m_v2 = edgeA.m_vertex2; m_v3 = edgeA.m_vertex3; bool hasVertex0 = edgeA.m_hasVertex0; bool hasVertex3 = edgeA.m_hasVertex3; edge1.set_Renamed(m_v2).subLocal(m_v1); edge1.normalize(); m_normal1.set_Renamed(edge1.y, -edge1.x); float offset1 = Vec2.dot(m_normal1, temp.set_Renamed(m_centroidB).subLocal(m_v1)); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { edge0.set_Renamed(m_v1).subLocal(m_v0); edge0.normalize(); m_normal0.set_Renamed(edge0.y, -edge0.x); convex1 = Vec2.cross(edge0, edge1) >= 0.0f; offset0 = Vec2.dot(m_normal0, temp.set_Renamed(m_centroidB).subLocal(m_v0)); } // Is there a following edge? if (hasVertex3) { edge2.set_Renamed(m_v3).subLocal(m_v2); edge2.normalize(); m_normal2.set_Renamed(edge2.y, -edge2.x); convex2 = Vec2.cross(edge1, edge2) > 0.0f; offset2 = Vec2.dot(m_normal2, temp.set_Renamed(m_centroidB).subLocal(m_v2)); } // Determine front or back collision. Determine collision normal limits. if (hasVertex0 && hasVertex3) { if (convex1 && convex2) { m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal0); m_upperLimit.set_Renamed(m_normal2); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } } else if (convex1) { m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal0); m_upperLimit.set_Renamed(m_normal1); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal2).negateLocal(); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } } else if (convex2) { m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal2); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal0).negateLocal(); } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal1); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal2).negateLocal(); m_upperLimit.set_Renamed(m_normal0).negateLocal(); } } } else if (hasVertex0) { if (convex1) { m_front = offset0 >= 0.0f || offset1 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal0); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal0).negateLocal(); } } } else if (hasVertex3) { if (convex2) { m_front = offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal2); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal1); } } else { m_front = offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal1); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal2).negateLocal(); m_upperLimit.set_Renamed(m_normal1); } } } else { m_front = offset1 >= 0.0f; if (m_front) { m_normal.set_Renamed(m_normal1); m_lowerLimit.set_Renamed(m_normal1).negateLocal(); m_upperLimit.set_Renamed(m_normal1).negateLocal(); } else { m_normal.set_Renamed(m_normal1).negateLocal(); m_lowerLimit.set_Renamed(m_normal1); m_upperLimit.set_Renamed(m_normal1); } } // Get polygonB in frameA m_polygonB.count = polygonB.m_count; for (int i = 0; i < polygonB.m_count; ++i) { Transform.mulToOutUnsafe(m_xf, polygonB.m_vertices[i], m_polygonB.vertices[i]); Rot.mulToOutUnsafe(m_xf.q, polygonB.m_normals[i], m_polygonB.normals[i]); } m_radius = 2.0f * Settings.polygonRadius; manifold.pointCount = 0; computeEdgeSeparation(edgeAxis); // If no valid normal can be found than this edge should not collide. if (edgeAxis.type == EPAxis.Type.UNKNOWN) { return; } if (edgeAxis.separation > m_radius) { return; } computePolygonSeparation(polygonAxis); if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius) { return; } // Use hysteresis for jitter reduction. float k_relativeTol = 0.98f; float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.type == EPAxis.Type.UNKNOWN) { primaryAxis = edgeAxis; } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } // ClipVertex[] ie = new ClipVertex[2]; if (primaryAxis.type == EPAxis.Type.EDGE_A) { manifold.type = Manifold.ManifoldType.FACE_A; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]); for (int i = 1; i < m_polygonB.count; ++i) { float value_Renamed = Vec2.dot(m_normal, m_polygonB.normals[i]); if (value_Renamed < bestValue) { bestValue = value_Renamed; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; ie[0].v.set_Renamed(m_polygonB.vertices[i1]); ie[0].id.indexA = 0; ie[0].id.indexB = (sbyte)i1; ie[0].id.typeA = (sbyte)ContactID.Type.FACE; ie[0].id.typeB = (sbyte)ContactID.Type.VERTEX; ie[1].v.set_Renamed(m_polygonB.vertices[i2]); ie[1].id.indexA = 0; ie[1].id.indexB = (sbyte)i2; ie[1].id.typeA = (sbyte)ContactID.Type.FACE; ie[1].id.typeB = (sbyte)ContactID.Type.VERTEX; if (m_front) { rf.i1 = 0; rf.i2 = 1; rf.v1.set_Renamed(m_v1); rf.v2.set_Renamed(m_v2); rf.normal.set_Renamed(m_normal1); } else { rf.i1 = 1; rf.i2 = 0; rf.v1.set_Renamed(m_v2); rf.v2.set_Renamed(m_v1); rf.normal.set_Renamed(m_normal1).negateLocal(); } } else { manifold.type = Manifold.ManifoldType.FACE_B; ie[0].v.set_Renamed(m_v1); ie[0].id.indexA = 0; ie[0].id.indexB = (sbyte)primaryAxis.index; ie[0].id.typeA = (sbyte)ContactID.Type.VERTEX; ie[0].id.typeB = (sbyte)ContactID.Type.FACE; ie[1].v.set_Renamed(m_v2); ie[1].id.indexA = 0; ie[1].id.indexB = (sbyte)primaryAxis.index; ie[1].id.typeA = (sbyte)ContactID.Type.VERTEX; ie[1].id.typeB = (sbyte)ContactID.Type.FACE; rf.i1 = primaryAxis.index; rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; rf.v1.set_Renamed(m_polygonB.vertices[rf.i1]); rf.v2.set_Renamed(m_polygonB.vertices[rf.i2]); rf.normal.set_Renamed(m_polygonB.normals[rf.i1]); } rf.sideNormal1.set_Renamed(rf.normal.y, -rf.normal.x); rf.sideNormal2.set_Renamed(rf.sideNormal1).negateLocal(); rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1); rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2); // Clip incident edge against extruded edge1 side edges. int np; // Clip to box side 1 np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); if (np < Settings.maxManifoldPoints) { return; } // Clip to negative box side 1 np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); if (np < Settings.maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == EPAxis.Type.EDGE_A) { manifold.localNormal.set_Renamed(rf.normal); manifold.localPoint.set_Renamed(rf.v1); } else { manifold.localNormal.set_Renamed(polygonB.m_normals[rf.i1]); manifold.localPoint.set_Renamed(polygonB.m_vertices[rf.i1]); } int pointCount = 0; for (int i = 0; i < Settings.maxManifoldPoints; ++i) { float separation; separation = Vec2.dot(rf.normal, temp.set_Renamed(clipPoints2[i].v).subLocal(rf.v1)); if (separation <= m_radius) { ManifoldPoint cp = manifold.points[pointCount]; if (primaryAxis.type == EPAxis.Type.EDGE_A) { // cp.localPoint = MulT(m_xf, clipPoints2[i].v); Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint); cp.id.set_Renamed(clipPoints2[i].id); } else { cp.localPoint.set_Renamed(clipPoints2[i].v); cp.id.typeA = clipPoints2[i].id.typeB; cp.id.typeB = clipPoints2[i].id.typeA; cp.id.indexA = clipPoints2[i].id.indexB; cp.id.indexB = clipPoints2[i].id.indexA; } ++pointCount; } } manifold.pointCount = pointCount; }
protected internal Contact(IWorldPool argPool) { m_fixtureA = null; m_fixtureB = null; m_nodeA = new ContactEdge(); m_nodeB = new ContactEdge(); m_manifold = new Manifold(); pool = argPool; }
public void Collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB) { Transform.MulTransToOutUnsafe(xfA, xfB, xf); Transform.MulToOutUnsafe(xf, polygonB.Centroid, centroidB); v0 = edgeA.Vertex0; v1 = edgeA.Vertex1; v2 = edgeA.Vertex2; v3 = edgeA.Vertex3; bool hasVertex0 = edgeA.HasVertex0; bool hasVertex3 = edgeA.HasVertex3; edge1.Set(v2).SubLocal(v1); edge1.Normalize(); normal1.Set(edge1.Y, -edge1.X); float offset1 = Vec2.Dot(normal1, temp.Set(centroidB).SubLocal(v1)); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { edge0.Set(v1).SubLocal(v0); edge0.Normalize(); normal0.Set(edge0.Y, -edge0.X); convex1 = Vec2.Cross(edge0, edge1) >= 0.0f; offset0 = Vec2.Dot(normal0, temp.Set(centroidB).SubLocal(v0)); } // Is there a following edge? if (hasVertex3) { edge2.Set(v3).SubLocal(v2); edge2.Normalize(); normal2.Set(edge2.Y, -edge2.X); convex2 = Vec2.Cross(edge1, edge2) > 0.0f; offset2 = Vec2.Dot(normal2, temp.Set(centroidB).SubLocal(v2)); } // Determine front or back collision. Determine collision normal limits. if (hasVertex0 && hasVertex3) { if (convex1 && convex2) { front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal0); upperLimit.Set(normal2); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal1).NegateLocal(); } } else if (convex1) { front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (front) { normal.Set(normal1); lowerLimit.Set(normal0); upperLimit.Set(normal1); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal2).NegateLocal(); upperLimit.Set(normal1).NegateLocal(); } } else if (convex2) { front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (front) { normal.Set(normal1); lowerLimit.Set(normal1); upperLimit.Set(normal2); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal0).NegateLocal(); } } else { front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal1); upperLimit.Set(normal1); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal2).NegateLocal(); upperLimit.Set(normal0).NegateLocal(); } } } else if (hasVertex0) { if (convex1) { front = offset0 >= 0.0f || offset1 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal0); upperLimit.Set(normal1).NegateLocal(); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1); upperLimit.Set(normal1).NegateLocal(); } } else { front = offset0 >= 0.0f && offset1 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal1); upperLimit.Set(normal1).NegateLocal(); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1); upperLimit.Set(normal0).NegateLocal(); } } } else if (hasVertex3) { if (convex2) { front = offset1 >= 0.0f || offset2 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal2); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal1); } } else { front = offset1 >= 0.0f && offset2 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal1); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal2).NegateLocal(); upperLimit.Set(normal1); } } } else { front = offset1 >= 0.0f; if (front) { normal.Set(normal1); lowerLimit.Set(normal1).NegateLocal(); upperLimit.Set(normal1).NegateLocal(); } else { normal.Set(normal1).NegateLocal(); lowerLimit.Set(normal1); upperLimit.Set(normal1); } } // Get polygonB in frameA this.polygonB.Count = polygonB.VertexCount; for (int i = 0; i < polygonB.VertexCount; ++i) { Transform.MulToOutUnsafe(xf, polygonB.Vertices[i], this.polygonB.Vertices[i]); Rot.MulToOutUnsafe(xf.Q, polygonB.Normals[i], this.polygonB.Normals[i]); } radius = 2.0f * Settings.POLYGON_RADIUS; manifold.PointCount = 0; ComputeEdgeSeparation(edgeAxis); // If no valid normal can be found than this edge should not collide. if (edgeAxis.Type == EPAxis.EPAxisType.Unknown) { return; } if (edgeAxis.Separation > radius) { return; } ComputePolygonSeparation(polygonAxis); if (polygonAxis.Type != EPAxis.EPAxisType.Unknown && polygonAxis.Separation > radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.Type == EPAxis.EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } // ClipVertex[] ie = new ClipVertex[2]; if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA) { manifold.Type = Manifold.ManifoldType.FaceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = Vec2.Dot(normal, this.polygonB.Normals[0]); for (int i = 1; i < this.polygonB.Count; ++i) { float value = Vec2.Dot(normal, this.polygonB.Normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < this.polygonB.Count ? i1 + 1 : 0; ie[0].V.Set(this.polygonB.Vertices[i1]); ie[0].Id.IndexA = 0; ie[0].Id.IndexB = (sbyte)i1; ie[0].Id.TypeA = (sbyte)ContactID.Type.Face; ie[0].Id.TypeB = (sbyte)ContactID.Type.Vertex; ie[1].V.Set(this.polygonB.Vertices[i2]); ie[1].Id.IndexA = 0; ie[1].Id.IndexB = (sbyte)i2; ie[1].Id.TypeA = (sbyte)ContactID.Type.Face; ie[1].Id.TypeB = (sbyte)ContactID.Type.Vertex; if (front) { rf.I1 = 0; rf.I2 = 1; rf.V1.Set(v1); rf.V2.Set(v2); rf.Normal.Set(normal1); } else { rf.I1 = 1; rf.I2 = 0; rf.V1.Set(v2); rf.V2.Set(v1); rf.Normal.Set(normal1).NegateLocal(); } } else { manifold.Type = Manifold.ManifoldType.FaceB; ie[0].V.Set(v1); ie[0].Id.IndexA = 0; ie[0].Id.IndexB = (sbyte)primaryAxis.Index; ie[0].Id.TypeA = (sbyte)ContactID.Type.Vertex; ie[0].Id.TypeB = (sbyte)ContactID.Type.Face; ie[1].V.Set(v2); ie[1].Id.IndexA = 0; ie[1].Id.IndexB = (sbyte)primaryAxis.Index; ie[1].Id.TypeA = (sbyte)ContactID.Type.Vertex; ie[1].Id.TypeB = (sbyte)ContactID.Type.Face; rf.I1 = primaryAxis.Index; rf.I2 = rf.I1 + 1 < this.polygonB.Count ? rf.I1 + 1 : 0; rf.V1.Set(this.polygonB.Vertices[rf.I1]); rf.V2.Set(this.polygonB.Vertices[rf.I2]); rf.Normal.Set(this.polygonB.Normals[rf.I1]); } rf.SideNormal1.Set(rf.Normal.Y, -rf.Normal.X); rf.SideNormal2.Set(rf.SideNormal1).NegateLocal(); rf.SideOffset1 = Vec2.Dot(rf.SideNormal1, rf.V1); rf.SideOffset2 = Vec2.Dot(rf.SideNormal2, rf.V2); // Clip incident edge against extruded edge1 side edges. int np; // Clip to box side 1 np = ClipSegmentToLine(clipPoints1, ie, rf.SideNormal1, rf.SideOffset1, rf.I1); if (np < Settings.MAX_MANIFOLD_POINTS) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(clipPoints2, clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.I2); if (np < Settings.MAX_MANIFOLD_POINTS) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA) { manifold.LocalNormal.Set(rf.Normal); manifold.LocalPoint.Set(rf.V1); } else { manifold.LocalNormal.Set(polygonB.Normals[rf.I1]); manifold.LocalPoint.Set(polygonB.Vertices[rf.I1]); } int pointCount = 0; for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++i) { float separation = Vec2.Dot(rf.Normal, temp.Set(clipPoints2[i].V).SubLocal(rf.V1)); if (separation <= radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA) { // cp.localPoint = MulT(m_xf, clipPoints2[i].v); Transform.MulTransToOutUnsafe(xf, clipPoints2[i].V, cp.LocalPoint); cp.Id.Set(clipPoints2[i].Id); } else { cp.LocalPoint.Set(clipPoints2[i].V); cp.Id.TypeA = clipPoints2[i].Id.TypeB; cp.Id.TypeB = clipPoints2[i].Id.TypeA; cp.Id.IndexA = clipPoints2[i].Id.IndexB; cp.Id.IndexB = clipPoints2[i].Id.IndexA; } ++pointCount; } } manifold.PointCount = pointCount; }
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB) { pool.getCollision().collideCircles(manifold, (CircleShape)m_fixtureA.Shape, xfA, (CircleShape)m_fixtureB.Shape, xfB); }
/// <summary> /// copies this manifold from the given one /// </summary> /// <param name="cp">manifold to copy from /// </param> public virtual void set_Renamed(Manifold cp) { for (int i = 0; i < cp.pointCount; i++) { points[i].set_Renamed(cp.points[i]); } type = cp.type; localNormal.set_Renamed(cp.localNormal); localPoint.set_Renamed(cp.localPoint); pointCount = cp.pointCount; }
/// <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(); } }
// 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); }
public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB) { if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case Manifold.ManifoldType.Circles: { // final Vec2 pointA = pool3; // final Vec2 pointB = pool4; // // normal.set(1, 0); // Transform.mulToOut(xfA, manifold.localPoint, pointA); // Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB); // // if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { // normal.set(pointB).subLocal(pointA); // normal.normalize(); // } // // cA.set(normal).mulLocal(radiusA).addLocal(pointA); // cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal(); // points[0].set(cA).addLocal(cB).mulLocal(0.5f); Vec2 pointA = pool3; Vec2 pointB = pool4; Normal.X = 1; Normal.Y = 0; // pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x * // manifold.localPoint.y; // pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y * // manifold.localPoint.y; // pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x * // manifold.points[0].localPoint.y; // pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y * // manifold.points[0].localPoint.y; Transform.MulToOut(xfA, manifold.LocalPoint, pointA); Transform.MulToOut(xfB, manifold.Points[0].LocalPoint, pointB); if (MathUtils.DistanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { Normal.X = pointB.X - pointA.X; Normal.Y = pointB.Y - pointA.Y; Normal.Normalize(); } float cAx = Normal.X * radiusA + pointA.X; float cAy = Normal.Y * radiusA + pointA.Y; float cBx = (-Normal.X) * radiusB + pointB.X; float cBy = (-Normal.Y) * radiusB + pointB.Y; Points[0].X = (cAx + cBx) * .5f; Points[0].Y = (cAy + cBy) * .5f; } break; case Manifold.ManifoldType.FaceA: { Vec2 planePoint = pool3; Rot.MulToOutUnsafe(xfA.Q, manifold.LocalNormal, Normal); Transform.MulToOut(xfA, manifold.LocalPoint, planePoint); Vec2 clipPoint = pool4; for (int i = 0; i < manifold.PointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cB = clipPoint - radiusB * normal; // points[i] = 0.5f * (cA + cB); Transform.MulToOut(xfB, manifold.Points[i].LocalPoint, clipPoint); // use cA as temporary for now // cA.set(clipPoint).subLocal(planePoint); // float scalar = radiusA - Vec2.dot(cA, normal); // cA.set(normal).mulLocal(scalar).addLocal(clipPoint); // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); float scalar = radiusA - ((clipPoint.X - planePoint.X) * Normal.X + (clipPoint.Y - planePoint.Y) * Normal.Y); float cAx = Normal.X * scalar + clipPoint.X; float cAy = Normal.Y * scalar + clipPoint.Y; float cBx = (-Normal.X) * radiusB + clipPoint.X; float cBy = (-Normal.Y) * radiusB + clipPoint.Y; Points[i].X = (cAx + cBx) * .5f; Points[i].Y = (cAy + cBy) * .5f; } } break; case Manifold.ManifoldType.FaceB: Vec2 planePoint2 = pool3; Rot.MulToOutUnsafe(xfB.Q, manifold.LocalNormal, Normal); Transform.MulToOut(xfB, manifold.LocalPoint, planePoint2); // final Mat22 R = xfB.q; // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y; // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y; // final Vec2 v = manifold.localPoint; // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; Vec2 clipPoint2 = pool4; for (int i = 0; i < manifold.PointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cA = clipPoint - radiusA * normal; // points[i] = 0.5f * (cA + cB); Transform.MulToOut(xfA, manifold.Points[i].LocalPoint, clipPoint2); // cB.set(clipPoint).subLocal(planePoint); // float scalar = radiusB - Vec2.dot(cB, normal); // cB.set(normal).mulLocal(scalar).addLocal(clipPoint); // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); // points[i] = 0.5f * (cA + cB); // // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x * // manifold.points[i].localPoint.y; // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y * // manifold.points[i].localPoint.y; float scalar = radiusB - ((clipPoint2.X - planePoint2.X) * Normal.X + (clipPoint2.Y - planePoint2.Y) * Normal.Y); float cBx = Normal.X * scalar + clipPoint2.X; float cBy = Normal.Y * scalar + clipPoint2.Y; float cAx = (-Normal.X) * radiusA + clipPoint2.X; float cAy = (-Normal.Y) * radiusA + clipPoint2.Y; Points[i].X = (cAx + cBx) * .5f; Points[i].Y = (cAy + cBy) * .5f; } // Ensure normal points from A to B. Normal.X = -Normal.X; Normal.Y = -Normal.Y; break; } }
/// <summary> /// copies this manifold from the given one /// </summary> /// <param name="cp">manifold to copy from</param> public void Set(Manifold cp) { for (int i = 0; i < cp.PointCount; i++) { Points[i].Set(cp.Points[i]); } Type = cp.Type; LocalNormal.Set(cp.LocalNormal); LocalPoint.Set(cp.LocalPoint); PointCount = cp.PointCount; }
/// <summary> /// Creates this manifold as a copy of the other /// </summary> /// <param name="other"></param> public Manifold(Manifold other) { Points = new ManifoldPoint[Settings.MAX_MANIFOLD_POINTS]; LocalNormal = other.LocalNormal.Clone(); LocalPoint = other.LocalPoint.Clone(); PointCount = other.PointCount; Type = other.Type; // djm: this is correct now for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; i++) { Points[i] = new ManifoldPoint(other.Points[i]); } }
public void initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB) { if (manifold.pointCount == 0) { return; } switch (manifold.type) { case Manifold.ManifoldType.CIRCLES: { // final Vec2 pointA = pool3; // final Vec2 pointB = pool4; // // normal.set(1, 0); // Transform.mulToOut(xfA, manifold.localPoint, pointA); // Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB); // // if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { // normal.set(pointB).subLocal(pointA); // normal.normalize(); // } // // cA.set(normal).mulLocal(radiusA).addLocal(pointA); // cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal(); // points[0].set(cA).addLocal(cB).mulLocal(0.5f); Vec2 pointA = pool3; Vec2 pointB = pool4; normal.x = 1; normal.y = 0; // pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x * // manifold.localPoint.y; // pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y * // manifold.localPoint.y; // pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x * // manifold.points[0].localPoint.y; // pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y * // manifold.points[0].localPoint.y; Transform.mulToOut(xfA, manifold.localPoint, pointA); Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB); if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { normal.x = pointB.x - pointA.x; normal.y = pointB.y - pointA.y; normal.normalize(); } float cAx = normal.x * radiusA + pointA.x; float cAy = normal.y * radiusA + pointA.y; float cBx = (-normal.x) * radiusB + pointB.x; float cBy = (-normal.y) * radiusB + pointB.y; points[0].x = (cAx + cBx) * .5f; points[0].y = (cAy + cBy) * .5f; } break; case Manifold.ManifoldType.FACE_A: { Vec2 planePoint = pool3; Rot.mulToOutUnsafe(xfA.q, manifold.localNormal, normal); Transform.mulToOut(xfA, manifold.localPoint, planePoint); Vec2 clipPoint = pool4; for (int i = 0; i < manifold.pointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cB = clipPoint - radiusB * normal; // points[i] = 0.5f * (cA + cB); Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint); // use cA as temporary for now // cA.set(clipPoint).subLocal(planePoint); // float scalar = radiusA - Vec2.dot(cA, normal); // cA.set(normal).mulLocal(scalar).addLocal(clipPoint); // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); float scalar = radiusA - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y); float cAx = normal.x * scalar + clipPoint.x; float cAy = normal.y * scalar + clipPoint.y; float cBx = (-normal.x) * radiusB + clipPoint.x; float cBy = (-normal.y) * radiusB + clipPoint.y; points[i].x = (cAx + cBx) * .5f; points[i].y = (cAy + cBy) * .5f; } } break; case Manifold.ManifoldType.FACE_B: Vec2 planePoint2 = pool3; Rot.mulToOutUnsafe(xfB.q, manifold.localNormal, normal); Transform.mulToOut(xfB, manifold.localPoint, planePoint2); // final Mat22 R = xfB.q; // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y; // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y; // final Vec2 v = manifold.localPoint; // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; Vec2 clipPoint2 = pool4; for (int i = 0; i < manifold.pointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cA = clipPoint - radiusA * normal; // points[i] = 0.5f * (cA + cB); Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint2); // cB.set(clipPoint).subLocal(planePoint); // float scalar = radiusB - Vec2.dot(cB, normal); // cB.set(normal).mulLocal(scalar).addLocal(clipPoint); // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); // points[i] = 0.5f * (cA + cB); // // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x * // manifold.points[i].localPoint.y; // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y * // manifold.points[i].localPoint.y; float scalar = radiusB - ((clipPoint2.x - planePoint2.x) * normal.x + (clipPoint2.y - planePoint2.y) * normal.y); float cBx = normal.x * scalar + clipPoint2.x; float cBy = normal.y * scalar + clipPoint2.y; float cAx = (-normal.x) * radiusA + clipPoint2.x; float cAy = (-normal.y) * radiusA + clipPoint2.y; points[i].x = (cAx + cBx) * .5f; points[i].y = (cAy + cBy) * .5f; } // Ensure normal points from A to B. normal.x = -normal.x; normal.y = -normal.y; break; } }
// 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(); }
/// <summary> /// Compute the point states given two manifolds. The states pertain to the transition from /// manifold1 to manifold2. So state1 is either persist or remove while state2 is either add or /// persist. /// </summary> /// <param name="state1"></param> /// <param name="state2"></param> /// <param name="manifold1"></param> /// <param name="manifold2"></param> public static void GetPointStates(PointState[] state1, PointState[] state2, Manifold manifold1, Manifold manifold2) { for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; i++) { state1[i] = PointState.NullState; state2[i] = PointState.NullState; } // Detect persists and removes. for (int i = 0; i < manifold1.PointCount; i++) { ContactID id = manifold1.Points[i].Id; state1[i] = PointState.RemoveState; for (int j = 0; j < manifold2.PointCount; j++) { if (manifold2.Points[j].Id.IsEqual(id)) { state1[i] = PointState.PersistState; break; } } } // Detect persists and adds for (int i = 0; i < manifold2.PointCount; i++) { ContactID id = manifold2.Points[i].Id; state2[i] = PointState.AddState; for (int j = 0; j < manifold1.PointCount; j++) { if (manifold1.Points[j].Id.IsEqual(id)) { state2[i] = PointState.PersistState; break; } } } }
/// <summary> /// Compute the point states given two manifolds. The states pertain to the transition from /// manifold1 to manifold2. So state1 is either persist or remove while state2 is either add or /// persist. /// </summary> /// <param name="state1"></param> /// <param name="state2"></param> /// <param name="manifold1"></param> /// <param name="manifold2"></param> public static void getPointStates(PointState[] state1, PointState[] state2, Manifold manifold1, Manifold manifold2) { for (int i = 0; i < Settings.maxManifoldPoints; i++) { state1[i] = PointState.NULL_STATE; state2[i] = PointState.NULL_STATE; } // Detect persists and removes. for (int i = 0; i < manifold1.pointCount; i++) { ContactID id = manifold1.points[i].id; state1[i] = PointState.REMOVE_STATE; for (int j = 0; j < manifold2.pointCount; j++) { if (manifold2.points[j].id.isEqual(id)) { state1[i] = PointState.PERSIST_STATE; break; } } } // Detect persists and adds for (int i = 0; i < manifold2.pointCount; i++) { ContactID id = manifold2.points[i].id; state2[i] = PointState.ADD_STATE; for (int j = 0; j < manifold1.pointCount; j++) { if (manifold1.points[j].id.isEqual(id)) { state2[i] = PointState.PERSIST_STATE; break; } } } }
/// <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 void evaluate(Manifold manifold, Transform xfA, Transform xfB) { ChainShape chain = (ChainShape)m_fixtureA.Shape; chain.getChildEdge(edge, m_indexA); pool.getCollision().collideEdgeAndPolygon(manifold, edge, xfA, (PolygonShape)m_fixtureB.Shape, xfB); }
public virtual void collideEdgeAndPolygon(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB) { collider.collide(manifold, edgeA, xfA, polygonB, xfB); }
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB) { ChainShape chain = (ChainShape)FixtureA.Shape; chain.GetChildEdge(edge, ChildIndexA); Pool.GetCollision().CollideEdgeAndPolygon(manifold, edge, xfA, (PolygonShape)FixtureB.Shape, xfB); }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold"></param> /// <param name="polygon1"></param> /// <param name="xf1"></param> /// <param name="polygon2"></param> /// <param name="xf2"></param> public void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) { // Find edge normal of max separation on A - return if separating axis is found // Find edge normal of max separation on B - return if separation axis is found // Choose reference edge as min(minA, minB) // Find incident edge // Clip // The normal points from 1 to 2 manifold.pointCount = 0; float totalRadius = polyA.m_radius + polyB.m_radius; findMaxSeparation(results1, polyA, xfA, polyB, xfB); if (results1.separation > totalRadius) { return; } findMaxSeparation(results2, polyB, xfB, polyA, xfA); if (results2.separation > totalRadius) { return; } PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge bool flip; float k_relativeTol = 0.98f; float k_absoluteTol = 0.001f; if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = results2.edgeIndex; manifold.type = Manifold.ManifoldType.FACE_B; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = results1.edgeIndex; manifold.type = Manifold.ManifoldType.FACE_A; flip = false; } findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.m_count; Vec2[] vertices1 = poly1.m_vertices; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; v11.set_Renamed(vertices1[iv1]); v12.set_Renamed(vertices1[iv2]); localTangent.set_Renamed(v12).subLocal(v11); localTangent.normalize(); Vec2.crossToOutUnsafe(localTangent, 1f, localNormal); // Vec2 localNormal = Vec2.cross(dv, // 1.0f); planePoint.set_Renamed(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11 // + v12); Rot.mulToOutUnsafe(xf1.q, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12 // - v11); Vec2.crossToOutUnsafe(tangent, 1f, normal); // Vec2 frontNormal = Vec2.cross(sideNormal, // 1.0f); Transform.mulToOut(xf1, v11, v11); Transform.mulToOut(xf1, v12, v12); // v11 = Mul(xf1, v11); // v12 = Mul(xf1, v12); // Face offset float frontOffset = Vec2.dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius; float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. // ClipVertex clipPoints1[2]; // ClipVertex clipPoints2[2]; int np; // Clip to box side 1 // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); tangent.negateLocal(); np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1); tangent.negateLocal(); if (np < 2) { return; } // Clip to negative box side 1 np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.localNormal.set_Renamed(localNormal); manifold.localPoint.set_Renamed(planePoint); int pointCount = 0; for (int i = 0; i < Settings.maxManifoldPoints; ++i) { float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.points[pointCount]; Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint); // cp.m_localPoint = MulT(xf2, clipPoints2[i].v); cp.id.set_Renamed(clipPoints2[i].id); if (flip) { // Swap features cp.id.flip(); } ++pointCount; } } manifold.pointCount = pointCount; }
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB) { pool.getCollision().collideEdgeAndPolygon(manifold, (EdgeShape)m_fixtureA.Shape, xfA, (PolygonShape)m_fixtureB.Shape, xfB); }
/// <summary> /// Creates this manifold as a copy of the other /// </summary> /// <param name="other"></param> public Manifold(Manifold other) { points = new ManifoldPoint[Settings.maxManifoldPoints]; localNormal = other.localNormal.Clone(); localPoint = other.localPoint.Clone(); pointCount = other.pointCount; type = other.type; // djm: this is correct now for (int i = 0; i < Settings.maxManifoldPoints; i++) { points[i] = new ManifoldPoint(other.points[i]); } }
public abstract void evaluate(Manifold manifold, Transform xfA, Transform xfB);
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB) { Pool.GetCollision().CollideEdgeAndPolygon(manifold, (EdgeShape)FixtureA.Shape, xfA, (PolygonShape)FixtureB.Shape, xfB); }
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB) { Pool.GetCollision().CollideCircles(manifold, (CircleShape)FixtureA.Shape, xfA, (CircleShape)FixtureB.Shape, xfB); }