public override void Update(GameSettings settings, GameTime gameTime) { Manifold manifold = new Manifold(); Collision.Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB); Vector2 normal; FixedArray2<Vector2> points; ContactSolver.WorldManifold.Initialize(ref manifold, ref _transformA, _polygonA.Radius, ref _transformB, _polygonB.Radius, out normal, out points); DrawString("Point count = " + manifold.PointCount); DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View); { Color color = new Color(0.9f, 0.9f, 0.9f); Vector2[] v = new Vector2[Settings.MaxPolygonVertices]; for (int i = 0; i < _polygonA.Vertices.Count; ++i) { v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]); } DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color); for (int i = 0; i < _polygonB.Vertices.Count; ++i) { v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]); } DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color); } for (int i = 0; i < manifold.PointCount; ++i) { DebugView.DrawPoint(points[i], 0.1f, new Color(0.9f, 0.3f, 0.3f)); DrawString(points[i].ToString()); } DebugView.EndCustomDraw(); }
public override void Update(GameSettings settings, GameTime gameTime) { Manifold manifold = new Manifold(); Collision.Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB); Vector2 normal; FixedArray2<Vector2> points; Collision.Collision.GetWorldManifold(ref manifold, ref _transformA, _polygonA.Radius, ref _transformB, _polygonB.Radius, out normal, out points); DebugView.DrawString(50, TextLine, "Point count = {0:n0}", manifold.PointCount); TextLine += 15; DebugView.BeginCustomDraw(); { Color color = new Color(0.9f, 0.9f, 0.9f); Vector2[] v = new Vector2[Settings.MaxPolygonVertices]; for (int i = 0; i < _polygonA.Vertices.Count; ++i) { v[i] = MathUtils.Multiply(ref _transformA, _polygonA.Vertices[i]); } DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color); for (int i = 0; i < _polygonB.Vertices.Count; ++i) { v[i] = MathUtils.Multiply(ref _transformB, _polygonB.Vertices[i]); } DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color); } for (int i = 0; i < manifold.PointCount; ++i) { DebugView.DrawPoint(points[i], 0.1f, new Color(0.9f, 0.3f, 0.3f)); } DebugView.EndCustomDraw(); }
private void PreSolve(Contact contact, ref Manifold oldManifold) { if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints) { Manifold manifold = contact.Manifold; if (manifold.PointCount == 0) return; Fixture fixtureA = contact.FixtureA; FixedArray2<PointState> state1, state2; Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); FixedArray2<Vector2> points; Vector2 normal; contact.GetWorldManifold(out normal, out points); for (int i = 0; i < manifold.PointCount && _pointCount < MaxContactPoints; ++i) { if (fixtureA == null) _points[i] = new ContactPoint(); ContactPoint cp = _points[_pointCount]; cp.Position = points[i]; cp.Normal = normal; cp.State = state2[i]; _points[_pointCount] = cp; ++_pointCount; } } }
protected override void PreSolve(Contact contact, ref Manifold oldManifold) { base.PreSolve(contact, ref oldManifold); Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; if (fixtureA == _platform) { contact.TangentSpeed = 5.0f; } if (fixtureB == _platform) { contact.TangentSpeed = -5.0f; } }
public bool Intersect(Fixture fixture, ref Vector2 point, out Feature result) { result = new Feature(); Manifold manifold = new Manifold(); Vector2 normal; FixedArray2<Vector2> points; Transform transformB = new Transform(); transformB.Set(Vector2.Zero, 0); circle.Position = point; PolygonShape polygonShape = fixture.Shape as PolygonShape; switch (fixture.ShapeType) { case ShapeType.Polygon: Collision.Collision.CollidePolygonAndCircle(ref manifold, polygonShape, ref fixture.Body.Xf, circle, ref transformB); ContactSolver.WorldManifold.Initialize(ref manifold, ref fixture.Body.Xf, polygonShape.Radius, ref transformB, circle.Radius, out normal, out points); result.Distance = (point-(fixture.Body.Position + new Vector2(1.5f, 1.5f))).Length(); break; default: throw new ArgumentOutOfRangeException(); } //result.Position = points[0]; //result.Normal = normal; //result.Distance = 1f; bool intersects = manifold.PointCount >= 1;//collision.Intersect(ref localPoint, out result); if (intersects) { result.Position = points[0]; result.Normal = normal; result.Distance *= -1; //Vector2 collisionPoint = points[0]; //result.Position = Body.GetWorldPoint(ref collisionPoint); //result.Normal = Body.GetWorldVector(ref normal); } return intersects; }
protected override void PreSolve(Contact contact, ref Manifold oldManifold) { base.PreSolve(contact, ref oldManifold); Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; if (fixtureA != _platform && fixtureA != _character) { return; } if (fixtureB != _platform && fixtureB != _character) { return; } Vector2 position = _character.Body.Position; if (position.Y < _top + _radius - 3.0f * Settings.LinearSlop) { contact.Enabled = false; } }
/// <summary> /// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points. /// Edge adjacency is handle to produce locally valid contact points and normals. This is intended /// to allow the polygon to slide smoothly over an edge chain. /// /// Algorithm /// 1. Classify front-side or back-side collision with edge. /// 2. Compute separation /// 3. Process adjacent edges /// 4. Classify adjacent edge as convex, flat, null, or concave /// 5. Skip null or concave edges. Concave edges get a separate manifold. /// 6. If the edge is flat, compute contact points as normal. Discard boundary points. /// 7. If the edge is convex, compute it's separation. /// 8. Use the minimum separation of up to three edges. If the minimum separation /// is not the primary edge, return. /// 9. If the minimum separation is the primary edge, compute the contact points and return. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="xfA">The xf A.</param> /// <param name="polygonB_in">The polygon b_in.</param> /// <param name="xfB">The xf B.</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB_in, ref Transform xfB) { manifold.PointCount = 0; Transform xf; MathUtils.MultiplyT(ref xfA, ref xfB, out xf); // Create a polygon for edge shape A s_polygonA.SetAsEdge(edgeA.Vertex1, edgeA.Vertex2); // Build polygonB in frame A s_polygonB.Radius = polygonB_in.Radius; s_polygonB.Centroid = MathUtils.Multiply(ref xf, polygonB_in.Centroid); s_polygonB.Vertices = new Vertices(polygonB_in.Vertices.Count); s_polygonB.Normals = new Vertices(polygonB_in.Vertices.Count); for (int i = 0; i < polygonB_in.Vertices.Count; ++i) { s_polygonB.Vertices.Add(MathUtils.Multiply(ref xf, polygonB_in.Vertices[i])); s_polygonB.Normals.Add(MathUtils.Multiply(ref xf.R, polygonB_in.Normals[i])); } float totalRadius = s_polygonA.Radius + s_polygonB.Radius; // Edge geometry Vector2 v1 = edgeA.Vertex1; Vector2 v2 = edgeA.Vertex2; Vector2 e = v2 - v1; Vector2 edgeNormal = new Vector2(e.Y, -e.X); edgeNormal.Normalize(); // Determine side bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB.Centroid - v1) >= 0.0f; if (isFrontSide == false) { edgeNormal = -edgeNormal; } // Compute primary separating axis EPAxis edgeAxis; ComputeEdgeSeperation(ref v1, ref edgeNormal, s_polygonB, out edgeAxis); if (edgeAxis.Separation > totalRadius) { // Shapes are separated return; } // Classify adjacent edges FixedArray2<EdgeType> types = new FixedArray2<EdgeType>(); //types[0] = EdgeType.Isolated; //types[1] = EdgeType.Isolated; if (edgeA.HasVertex0) { Vector2 v0 = edgeA.Vertex0; float s = Vector2.Dot(edgeNormal, v0 - v1); if (s > 0.1f * Settings.LinearSlop) { types[0] = EdgeType.Concave; } else if (s >= -0.1f * Settings.LinearSlop) { types[0] = EdgeType.Flat; } else { types[0] = EdgeType.Convex; } } if (edgeA.HasVertex3) { Vector2 v3 = edgeA.Vertex3; float s = Vector2.Dot(edgeNormal, v3 - v2); if (s > 0.1f * Settings.LinearSlop) { types[1] = EdgeType.Concave; } else if (s >= -0.1f * Settings.LinearSlop) { types[1] = EdgeType.Flat; } else { types[1] = EdgeType.Convex; } } if (types[0] == EdgeType.Convex) { // Check separation on previous edge. Vector2 v0 = edgeA.Vertex0; Vector2 e0 = v1 - v0; Vector2 n0 = new Vector2(e0.Y, -e0.X); n0.Normalize(); if (isFrontSide == false) { n0 = -n0; } EPAxis axis1; ComputeEdgeSeperation(ref v0, ref n0, s_polygonB, out axis1); if (axis1.Separation > edgeAxis.Separation) { // The polygon should collide with previous edge return; } } if (types[1] == EdgeType.Convex) { // Check separation on next edge. Vector2 v3 = edgeA.Vertex3; Vector2 e2 = v3 - v2; Vector2 n2 = new Vector2(e2.Y, -e2.X); n2.Normalize(); if (isFrontSide == false) { n2 = -n2; } EPAxis axis2; ComputeEdgeSeperation(ref v2, ref n2, s_polygonB, out axis2); if (axis2.Separation > edgeAxis.Separation) { // The polygon should collide with the next edge return; } } EPAxis polygonAxis; ComputePolygonSeperation(ref v1, ref v2, s_polygonB, totalRadius, out polygonAxis); if (polygonAxis.Separation > totalRadius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } PolygonShape poly1; PolygonShape poly2; if (primaryAxis.Type == EPAxisType.EdgeA) { poly1 = s_polygonA; poly2 = s_polygonB; if (isFrontSide == false) { primaryAxis.Index = 1; } manifold.Type = ManifoldType.FaceA; } else { poly1 = s_polygonB; poly2 = s_polygonA; manifold.Type = ManifoldType.FaceB; } int edge1 = primaryAxis.Index; FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>(); FindIncidentEdge(ref incidentEdge, poly1, primaryAxis.Index, poly2); int count1 = poly1.Vertices.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = poly1.Vertices[iv1]; Vector2 v12 = poly1.Vertices[iv2]; Vector2 tangent = v12 - v11; tangent.Normalize(); Vector2 normal = MathUtils.Cross(tangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; // Clip to box side 1 int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = normal; manifold.LocalPoint = planePoint; } else { manifold.LocalNormal = MathUtils.MultiplyT(ref xf.R, normal); manifold.LocalPoint = MathUtils.MultiplyT(ref xf, planePoint); } int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MultiplyT(ref xf, clipPoints2[i].V); cp.Id = clipPoints2[i].ID; } else { cp.LocalPoint = clipPoints2[i].V; cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB; cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA; cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB; cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA; } manifold.Points[pointCount] = cp; if (cp.Id.Features.TypeA == (byte) ContactFeatureType.Vertex && types[cp.Id.Features.IndexA] == EdgeType.Flat) { continue; } ++pointCount; } } manifold.PointCount = pointCount; }
protected virtual void PreSolve(Contact contact, ref Manifold oldManifold) { }
public void PreSolve(Contact contact, ref Manifold manifold) { EventSystem eventSystem = (EventSystem)_systemManager.getSystem(SystemType.Event); Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; int playerId = PlayerSystem.PLAYER_ID; int entityA = (int)fixtureA.Body.UserData; int entityB = (int)fixtureB.Body.UserData; string levelUid = LevelSystem.currentLevelUid; // Check for custom collision filters bool fixtureAIgnoresEntityB = fixtureA.IsIgnoredEntity(entityB); bool fixtureBIgnoresEntityA = fixtureB.IsIgnoredEntity(entityA); if (fixtureAIgnoresEntityB) contact.Enabled = false; else if (fixtureBIgnoresEntityA) contact.Enabled = false; // Check for item pickup if (contact.IsTouching() && (entityA == playerId || entityB == playerId)) { int itemEntityId = entityA == playerId ? entityB : entityA; Fixture fixture = entityA == playerId ? fixtureB : fixtureA; ItemComponent itemComponent = _entityManager.getComponent(levelUid, itemEntityId, ComponentType.Item) as ItemComponent; if (itemComponent != null) { contact.Enabled = false; if (itemComponent.state.inWorld) { InventoryComponent playerInventory = _entityManager.getComponent(levelUid, playerId, ComponentType.Inventory) as InventoryComponent; EquipmentSystem equipmentSystem = _systemManager.getSystem(SystemType.Equipment) as EquipmentSystem; equipmentSystem.addInventoryItem(playerInventory, itemComponent); itemComponent.state.inWorld = false; _bodiesToRemove.Add(fixture.Body); _entityManager.killEntity(levelUid, itemEntityId); eventSystem.postEvent(new GameEvent(GameEventType.OnItemPickedUp, itemEntityId)); } } } }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polyA">The poly A.</param> /// <param name="transformA">The transform A.</param> /// <param name="polyB">The poly B.</param> /// <param name="transformB">The transform B.</param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform transformA, PolygonShape polyB, ref Transform transformB) { manifold.PointCount = 0; float totalRadius = polyA.Radius + polyB.Radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB); if (separationA > totalRadius) return; int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA); if (separationB > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge bool flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = transformB; xf2 = transformA; edge1 = edgeB; manifold.Type = ManifoldType.FaceB; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = transformA; xf2 = transformB; edge1 = edgeA; manifold.Type = ManifoldType.FaceA; flip = false; } FixedArray2<ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1.Vertices.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = poly1.Vertices[iv1]; Vector2 v12 = poly1.Vertices[iv2]; float localTangentX = v12.X - v11.X; float localTangentY = v12.Y - v11.Y; float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY); localTangentX = localTangentX * factor; localTangentY = localTangentY * factor; Vector2 localNormal = new Vector2(localTangentY, -localTangentX); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY, xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY); float normalx = tangent.Y; float normaly = -tangent.X; v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y, xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y); v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y, xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y); // Face offset. float frontOffset = normalx * v11.X + normaly * v11.Y; // Side offsets, extended by polytope skin thickness. float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; // Clip to box side 1 int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.LocalNormal = localNormal; manifold.LocalPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { Vector2 value = clipPoints2[i].V; float separation = normalx * value.X + normaly * value.Y - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; Vector2 tmp = clipPoints2[i].V; float tmp1X = tmp.X - xf2.Position.X; float tmp1Y = tmp.Y - xf2.Position.Y; cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y; cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y; cp.Id = clipPoints2[i].ID; if (flip) { // Swap features ContactFeature cf = cp.Id.Features; cp.Id.Features.IndexA = cf.IndexB; cp.Id.Features.IndexB = cf.IndexA; cp.Id.Features.TypeA = cf.TypeB; cp.Id.Features.TypeB = cf.TypeA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }
/// Compute the collision manifold between two circles. public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold.PointCount = 0; float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y; float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y; float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y; float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y; float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy); float radius = circleA.Radius + circleB.Radius; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circleA.Position; manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="transformB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> /// <param name="normal">World vector pointing from A to B</param> /// <param name="points">Torld contact point (point of intersection).</param> public static void GetWorldManifold(ref Manifold manifold, ref Transform transformA, float radiusA, ref Transform transformB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points) { points = new FixedArray2<Vector2>(); normal = Vector2.Zero; if (manifold.PointCount == 0) { normal = Vector2.UnitY; return; } switch (manifold.Type) { case ManifoldType.Circles: { Vector2 tmp = manifold.Points[0].LocalPoint; float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + transformA.R.Col2.X * manifold.LocalPoint.Y; float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + transformA.R.Col2.Y * manifold.LocalPoint.Y; float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X + transformB.R.Col2.X * tmp.Y; float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + transformB.R.Col2.Y * tmp.Y; normal.X = 1; normal.Y = 0; float result = (pointAx - pointBx) * (pointAx - pointBx) + (pointAy - pointBy) * (pointAy - pointBy); if (result > Settings.Epsilon * Settings.Epsilon) { float tmpNormalx = pointBx - pointAx; float tmpNormaly = pointBy - pointAy; float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly); normal.X = tmpNormalx * factor; normal.Y = tmpNormaly * factor; } Vector2 c = Vector2.Zero; c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X); c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y); points[0] = 0.5f * c; } break; case ManifoldType.FaceA: { normal.X = transformA.R.Col1.X * manifold.LocalNormal.X + transformA.R.Col2.X * manifold.LocalNormal.Y; normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X + transformA.R.Col2.Y * manifold.LocalNormal.Y; float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + transformA.R.Col2.X * manifold.LocalPoint.Y; float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + transformA.R.Col2.Y * manifold.LocalPoint.Y; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 tmp = manifold.Points[i].LocalPoint; float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X + transformB.R.Col2.X * tmp.Y; float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + transformB.R.Col2.Y * tmp.Y; float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; Vector2 c = Vector2.Zero; c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X); c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y); points[i] = 0.5f * c; } } break; case ManifoldType.FaceB: { normal.X = transformB.R.Col1.X * manifold.LocalNormal.X + transformB.R.Col2.X * manifold.LocalNormal.Y; normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X + transformB.R.Col2.Y * manifold.LocalNormal.Y; float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X + transformB.R.Col2.X * manifold.LocalPoint.Y; float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X + transformB.R.Col2.Y * manifold.LocalPoint.Y; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 tmp = manifold.Points[i].LocalPoint; float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X + transformA.R.Col2.X * tmp.Y; float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X + transformA.R.Col2.Y * tmp.Y; float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; Vector2 c = Vector2.Zero; c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X); c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y); points[i] = 0.5f * c; } // Ensure normal points from A to B. normal *= -1; } break; default: normal = Vector2.UnitY; break; } }
/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="transformA">The transform A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform B.</param> public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle in frame of edge Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position)); Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2; Vector2 e = B - A; // Barycentric coordinates float u = Vector2.Dot(e, B - Q); float v = Vector2.Dot(e, Q - A); float radius = edgeA.Radius + circleB.Radius; ContactFeature cf; cf.IndexB = 0; cf.TypeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if (v <= 0.0f) { P = A; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.HasVertex0) { Vector2 A1 = edgeA.Vertex0; Vector2 B1 = A; Vector2 e1 = B1 - A1; float u1 = Vector2.Dot(e1, B1 - Q); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region B if (u <= 0.0f) { P = B; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.HasVertex3) { Vector2 B2 = edgeA.Vertex3; Vector2 A2 = B; Vector2 e2 = B2 - A2; float v2 = Vector2.Dot(e2, Q - A2); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.IndexA = 1; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region AB float den; Vector2.Dot(ref e, ref e, out den); Debug.Assert(den > 0.0f); P = (1.0f / den) * (u * A + v * B); d = Q - P; float dd2; Vector2.Dot(ref d, ref d, out dd2); if (dd2 > radius * radius) { return; } Vector2 n = new Vector2(-e.Y, e.X); if (Vector2.Dot(n, Q - A) < 0.0f) { n = new Vector2(-n.X, -n.Y); } n.Normalize(); cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Face; manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = n; manifold.LocalPoint = A; ManifoldPoint mp2 = new ManifoldPoint(); mp2.Id.Key = 0; mp2.Id.Features = cf; mp2.LocalPoint = circleB.Position; manifold.Points[0] = mp2; }
/// <summary> /// Compute the collision manifold between two circles. /// </summary> public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold.PointCount = 0; Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position); Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position); Vector2 d = pB - pA; float distSqr = Vector2.Dot(d, d); float radius = circleA.Radius + circleB.Radius; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circleA.Position; manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; }
/// <summary> /// Collides and edge and a polygon, taking into account edge adjacency. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="xfA">The xf A.</param> /// <param name="polygonB">The polygon B.</param> /// <param name="xfB">The xf B.</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { EPCollider collider = new EPCollider(); collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB); }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="transformB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> public WorldManifold(ref Manifold manifold, ref Transform transformA, float radiusA, ref Transform transformB, float radiusB) { Points = new FixedArray2<Vector2>(); if (manifold.PointCount == 0) { Normal = Vector2.UnitY; return; } switch (manifold.Type) { case ManifoldType.Circles: { Vector2 pointA = MathUtils.Multiply(ref transformA, manifold.LocalPoint); Vector2 pointB = MathUtils.Multiply(ref transformB, manifold.Points[0].LocalPoint); Normal = new Vector2(1.0f, 0.0f); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { Normal = pointB - pointA; Normal.Normalize(); } Vector2 cA = pointA + radiusA * Normal; Vector2 cB = pointB - radiusB * Normal; Points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { Normal = MathUtils.Multiply(ref transformA.R, manifold.LocalNormal); Vector2 planePoint = MathUtils.Multiply(ref transformA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Multiply(ref transformB, manifold.Points[i].LocalPoint); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, Normal)) * Normal; Vector2 cB = clipPoint - radiusB * Normal; Points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { Normal = MathUtils.Multiply(ref transformB.R, manifold.LocalNormal); Vector2 planePoint = MathUtils.Multiply(ref transformB, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Multiply(ref transformA, manifold.Points[i].LocalPoint); Vector2 cA = clipPoint - radiusA * Normal; Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, Normal)) * Normal; Points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. Normal *= -1; } break; default: Normal = Vector2.UnitY; break; } }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polyA">The poly A.</param> /// <param name="transformA">The transform A.</param> /// <param name="polyB">The poly B.</param> /// <param name="transformB">The transform B.</param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform transformA, PolygonShape polyB, ref Transform transformB) { manifold.PointCount = 0; float totalRadius = polyA.Radius + polyB.Radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB); if (separationA > totalRadius) return; int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA); if (separationB > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge bool flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = transformB; xf2 = transformA; edge1 = edgeB; manifold.Type = ManifoldType.FaceB; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = transformA; xf2 = transformB; edge1 = edgeA; manifold.Type = ManifoldType.FaceA; flip = false; } FixedArray2<ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1.Vertices.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = poly1.Vertices[iv1]; Vector2 v12 = poly1.Vertices[iv2]; Vector2 localTangent = v12 - v11; localTangent.Normalize(); Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent); Vector2 normal = MathUtils.Cross(tangent, 1.0f); v11 = MathUtils.Multiply(ref xf1, v11); v12 = MathUtils.Multiply(ref xf1, v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.LocalNormal = localNormal; manifold.LocalPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].V); cp.Id = clipPoints2[i].ID; if (flip) { // Swap features ContactFeature cf = cp.Id.Features; cp.Id.Features.IndexA = cf.IndexB; cp.Id.Features.IndexB = cf.IndexA; cp.Id.Features.TypeA = cf.TypeB; cp.Id.Features.TypeB = cf.TypeA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }
/// <summary> /// Compute the collision manifold between a polygon and a circle. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polygonA">The polygon A.</param> /// <param name="transformA">The transform of A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform of B.</param> public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = MathUtils.Multiply(ref transformB, circleB.Position); Vector2 cLocal = MathUtils.MultiplyT(ref transformA, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.MaxFloat; float radius = polygonA.Radius + circleB.Radius; int vertexCount = polygonA.Vertices.Count; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.Dot(polygonA.Normals[i], cLocal - polygonA.Vertices[i]); 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; Vector2 v1 = polygonA.Vertices[vertIndex1]; Vector2 v2 = polygonA.Vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.Epsilon) { manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[normalIndex]; manifold.LocalPoint = 0.5f * (v1 + v2); var p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; return; } // Compute barycentric coordinates float u1 = Vector2.Dot(cLocal - v1, v2 - v1); float u2 = Vector2.Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v1; manifold.LocalNormal.Normalize(); manifold.LocalPoint = v1; var p0b = manifold.Points[0]; p0b.LocalPoint = circleB.Position; p0b.Id.Key = 0; manifold.Points[0] = p0b; } else if (u2 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v2; manifold.LocalNormal.Normalize(); manifold.LocalPoint = v2; var p0c = manifold.Points[0]; p0c.LocalPoint = circleB.Position; p0c.Id.Key = 0; manifold.Points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); float separation2 = Vector2.Dot(cLocal - faceCenter, polygonA.Normals[vertIndex1]); if (separation2 > radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[vertIndex1]; manifold.LocalPoint = faceCenter; var p0d = manifold.Points[0]; p0d.LocalPoint = circleB.Position; p0d.Id.Key = 0; manifold.Points[0] = p0d; } }
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { // Algorithm: // 1. Classify v1 and v2 // 2. Classify polygon centroid as front or back // 3. Flip normal if necessary // 4. Initialize normal range to [-pi, pi] about face normal // 5. Adjust normal range according to adjacent edges // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip _xf = MathUtils.MulT(xfA, xfB); _centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid); _v0 = edgeA.Vertex0; _v1 = edgeA._vertex1; _v2 = edgeA._vertex2; _v3 = edgeA.Vertex3; bool hasVertex0 = edgeA.HasVertex0; bool hasVertex3 = edgeA.HasVertex3; Vector2 edge1 = _v2 - _v1; edge1.Normalize(); _normal1 = new Vector2(edge1.Y, -edge1.X); float offset1 = Vector2.Dot(_normal1, _centroidB - _v1); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { Vector2 edge0 = _v1 - _v0; edge0.Normalize(); _normal0 = new Vector2(edge0.Y, -edge0.X); convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f; offset0 = Vector2.Dot(_normal0, _centroidB - _v0); } // Is there a following edge? if (hasVertex3) { Vector2 edge2 = _v3 - _v2; edge2.Normalize(); _normal2 = new Vector2(edge2.Y, -edge2.X); convex2 = MathUtils.Cross(edge1, edge2) > 0.0f; offset2 = Vector2.Dot(_normal2, _centroidB - _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 = _normal1; _lowerLimit = _normal0; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = -_normal1; } } else if (convex1) { _front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (_front) { _normal = _normal1; _lowerLimit = _normal0; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = -_normal1; } } else if (convex2) { _front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = -_normal0; } } else { _front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = -_normal0; } } } else if (hasVertex0) { if (convex1) { _front = offset0 >= 0.0f || offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal0; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = -_normal1; } } else { _front = offset0 >= 0.0f && offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = _normal1; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = -_normal0; } } } else if (hasVertex3) { if (convex2) { _front = offset1 >= 0.0f || offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = _normal2; } else { _normal = -_normal1; _lowerLimit = -_normal1; _upperLimit = _normal1; } } else { _front = offset1 >= 0.0f && offset2 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = _normal1; } else { _normal = -_normal1; _lowerLimit = -_normal2; _upperLimit = _normal1; } } } else { _front = offset1 >= 0.0f; if (_front) { _normal = _normal1; _lowerLimit = -_normal1; _upperLimit = -_normal1; } else { _normal = -_normal1; _lowerLimit = _normal1; _upperLimit = _normal1; } } // Get polygonB in frameA _polygonB.Count = polygonB.Vertices.Count; for (int i = 0; i < polygonB.Vertices.Count; ++i) { _polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]); _polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]); } _radius = 2.0f * Settings.PolygonRadius; manifold.PointCount = 0; EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. if (edgeAxis.Type == EPAxisType.Unknown) { return; } if (edgeAxis.Separation > _radius) { return; } EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.Type != 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 == EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } FixedArray2<ClipVertex> ie = new FixedArray2<ClipVertex>(); ReferenceFace rf; if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.Type = ManifoldType.FaceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]); for (int i = 1; i < _polygonB.Count; ++i) { float value = Vector2.Dot(_normal, _polygonB.Normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0; ClipVertex c0 = ie[0]; c0.V = _polygonB.Vertices[i1]; c0.ID.Features.IndexA = 0; c0.ID.Features.IndexB = (byte)i1; c0.ID.Features.TypeA = (byte)ContactFeatureType.Face; c0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; ie[0] = c0; ClipVertex c1 = ie[1]; c1.V = _polygonB.Vertices[i2]; c1.ID.Features.IndexA = 0; c1.ID.Features.IndexB = (byte)i2; c1.ID.Features.TypeA = (byte)ContactFeatureType.Face; c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; ie[1] = c1; if (_front) { rf.i1 = 0; rf.i2 = 1; rf.v1 = _v1; rf.v2 = _v2; rf.normal = _normal1; } else { rf.i1 = 1; rf.i2 = 0; rf.v1 = _v2; rf.v2 = _v1; rf.normal = -_normal1; } } else { manifold.Type = ManifoldType.FaceB; ClipVertex c0 = ie[0]; c0.V = _v1; c0.ID.Features.IndexA = 0; c0.ID.Features.IndexB = (byte)primaryAxis.Index; c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; c0.ID.Features.TypeB = (byte)ContactFeatureType.Face; ie[0] = c0; ClipVertex c1 = ie[1]; c1.V = _v2; c1.ID.Features.IndexA = 0; c1.ID.Features.IndexB = (byte)primaryAxis.Index; c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; c1.ID.Features.TypeB = (byte)ContactFeatureType.Face; ie[1] = c1; rf.i1 = primaryAxis.Index; rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0; rf.v1 = _polygonB.Vertices[rf.i1]; rf.v2 = _polygonB.Vertices[rf.i2]; rf.normal = _polygonB.Normals[rf.i1]; } rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X); rf.sideNormal2 = -rf.sideNormal1; rf.sideOffset1 = Vector2.Dot(rf.sideNormal1, rf.v1); rf.sideOffset2 = Vector2.Dot(rf.sideNormal2, rf.v2); // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref ie, rf.sideNormal1, rf.sideOffset1, rf.i1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = rf.normal; manifold.LocalPoint = rf.v1; } else { manifold.LocalNormal = polygonB.Normals[rf.i1]; manifold.LocalPoint = polygonB.Vertices[rf.i1]; } int pointCount = 0; for (int i = 0; i < Settings.MaxManifoldPoints; ++i) { float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1); if (separation <= _radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V); cp.Id = clipPoints2[i].ID; } else { cp.LocalPoint = clipPoints2[i].V; cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB; cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA; cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB; cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }
bool OnCollision(Fixture fixtureA, Fixture fixtureB, Contact contact) { Vector2 movementBefore = body.LinearVelocity; body.LinearVelocity = new Vector2(body.LinearVelocity.X, 0); Block block=null; foreach (Entity e in game.entities) { if (e is Block) { Block b = (Block)e; if(b.fixture.Equals(fixtureB)) { block = b; } } } if (contact.IsTouching()) { if (isMagnet(fixtureB)) return true; Manifold colis = new Manifold(); contact.GetManifold(out colis); Vector2 pColis = colis.LocalNormal; if (pColis.X == 0 && pColis.Y == 1) { onGround = true; modes = Modes.GROUND; return true; } if (pColis.X != 0 && pColis.Y == 0) { collisionRec = getBlockFromFixture(fixtureB); if (fixtureB.Body.Rotation % 45 != 0) { onGround = true; modes = Modes.GROUND; return false; } if (onGround) { modes = Modes.WALL; return false; } float direction = inputState.GetJoyDirection(); float x = (float)Math.Sin(direction); if (pColis.X > 0) isWallOnRight = true; else isWallOnRight = false; float xMomentum = 0; if (movementBefore.Y > 0) { xMomentum = Math.Abs(body.LinearVelocity.X * 0.3f); } else if (movementBefore.Y < 0) { xMomentum = -Math.Abs(body.LinearVelocity.X * 0.8f); } XboxInput xbi = (XboxInput)inputState; if (xbi.getYDirection() < 0 || Keyboard.GetState().IsKeyDown(Keys.Up) || Keyboard.GetState().IsKeyDown(Keys.W)) xMomentum += xbi.getYDirection() * 4f; else xMomentum = 0; body.LinearVelocity = new Vector2(0, body.LinearVelocity.Y + xMomentum); //body.IgnoreGravity = true; onGround = true; onGround = true; modes = Modes.WALL; ignoreInput = 2; return false; } } return true; }
void onPreSolve(Contact contact, ref Manifold oldManifold) { // ... }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="xfA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="xfB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> /// <param name="normal">World vector pointing from A to B</param> /// <param name="points">Torld contact point (point of intersection).</param> public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points) { normal = Vector2.Zero; points = new FixedArray2<Vector2>(); if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { normal = new Vector2(1.0f, 0.0f); Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { normal = pointB - pointA; normal.Normalize(); } Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. normal = -normal; } break; } }
/// <summary> /// Collides and edge and a polygon, taking into account edge adjacency. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="xfA">The xf A.</param> /// <param name="polygonB">The polygon B.</param> /// <param name="xfB">The xf B.</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { MathUtils.MultiplyT(ref xfA, ref xfB, out _xf); // Edge geometry _edgeA.V0 = edgeA.Vertex0; _edgeA.V1 = edgeA.Vertex1; _edgeA.V2 = edgeA.Vertex2; _edgeA.V3 = edgeA.Vertex3; Vector2 e = _edgeA.V2 - _edgeA.V1; // Normal points outwards in CCW order. _edgeA.Normal = new Vector2(e.Y, -e.X); _edgeA.Normal.Normalize(); _edgeA.HasVertex0 = edgeA.HasVertex0; _edgeA.HasVertex3 = edgeA.HasVertex3; // Proxy for edge _proxyA.Vertices[0] = _edgeA.V1; _proxyA.Vertices[1] = _edgeA.V2; _proxyA.Normals[0] = _edgeA.Normal; _proxyA.Normals[1] = -_edgeA.Normal; _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2); _proxyA.Count = 2; // Proxy for polygon _proxyB.Count = polygonB.Vertices.Count; _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid); for (int i = 0; i < polygonB.Vertices.Count; ++i) { _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]); _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]); } _radius = 2.0f * Settings.PolygonRadius; _limit11 = Vector2.Zero; _limit12 = Vector2.Zero; _limit21 = Vector2.Zero; _limit22 = Vector2.Zero; //Collide(ref manifold); inline start manifold.PointCount = 0; //ComputeAdjacency(); inline start Vector2 v0 = _edgeA.V0; Vector2 v1 = _edgeA.V1; Vector2 v2 = _edgeA.V2; Vector2 v3 = _edgeA.V3; // Determine allowable the normal regions based on adjacency. // Note: it may be possible that no normal is admissable. Vector2 centerB = _proxyB.Centroid; if (_edgeA.HasVertex0) { Vector2 e0 = v1 - v0; Vector2 e1 = v2 - v1; Vector2 n0 = new Vector2(e0.Y, -e0.X); Vector2 n1 = new Vector2(e1.Y, -e1.X); n0.Normalize(); n1.Normalize(); bool convex = MathUtils.Cross(n0, n1) >= 0.0f; bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; if (convex) { if (front0 || front1) { _limit11 = n1; _limit12 = n0; } else { _limit11 = -n1; _limit12 = -n0; } } else { if (front0 && front1) { _limit11 = n0; _limit12 = n1; } else { _limit11 = -n0; _limit12 = -n1; } } } else { _limit11 = Vector2.Zero; _limit12 = Vector2.Zero; } if (_edgeA.HasVertex3) { Vector2 e1 = v2 - v1; Vector2 e2 = v3 - v2; Vector2 n1 = new Vector2(e1.Y, -e1.X); Vector2 n2 = new Vector2(e2.Y, -e2.X); n1.Normalize(); n2.Normalize(); bool convex = MathUtils.Cross(n1, n2) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f; if (convex) { if (front1 || front2) { _limit21 = n2; _limit22 = n1; } else { _limit21 = -n2; _limit22 = -n1; } } else { if (front1 && front2) { _limit21 = n1; _limit22 = n2; } else { _limit21 = -n1; _limit22 = -n2; } } } else { _limit21 = Vector2.Zero; _limit22 = Vector2.Zero; } //ComputeAdjacency(); inline end //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. // This can happen on the middle edge of a 3-edge zig-zag chain. if (edgeAxis.Type == EPAxisType.Unknown) { return; } if (edgeAxis.Separation > _radius) { return; } EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.Type != 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 == EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } EPProxy proxy1; EPProxy proxy2; FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>(); if (primaryAxis.Type == EPAxisType.EdgeA) { proxy1 = _proxyA; proxy2 = _proxyB; manifold.Type = ManifoldType.FaceA; } else { proxy1 = _proxyB; proxy2 = _proxyA; manifold.Type = ManifoldType.FaceB; } int edge1 = primaryAxis.Index; FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2); int count1 = proxy1.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = proxy1.Vertices[iv1]; Vector2 v12 = proxy1.Vertices[iv2]; Vector2 tangent = v12 - v11; tangent.Normalize(); Vector2 normal = MathUtils.Cross(tangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius; float sideOffset2 = Vector2.Dot(tangent, v12) + _radius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = normal; manifold.LocalPoint = planePoint; } else { manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal); manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint); } int pointCount = 0; for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1) { float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset; if (separation <= _radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V); cp.Id = clipPoints2[i1].ID; } else { cp.LocalPoint = clipPoints2[i1].V; cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB; cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA; cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB; cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; //Collide(ref manifold); inline end }
private void PreSolveListener(Contact contact, ref Manifold oldManifold) { if (contact.IsTouching) { Debug.Assert(contact.Manifold.PointCount > 0); /* Sometimes a body will tunnel through a portal fixture. * The circumstances aren't well understood but checking if the contact normal * is facing from fixtureB to fixtureA (it should always face from A to B * according to the Box2D documentation) and then reversing the normal if it is * helps prevent tunneling from occuring.*/ if (!FixtureExt.GetData(contact.FixtureA).IsPortalParentless() || !FixtureExt.GetData(contact.FixtureB).IsPortalParentless()) { Xna.Vector2 normal; FixedArray2<Xna.Vector2> vList; contact.GetWorldManifold(out normal, out vList); Vector2 center0 = FixtureExt.GetCenterWorld(contact.FixtureA); Vector2 center1 = FixtureExt.GetCenterWorld(contact.FixtureB); if (Math.Abs(MathExt.AngleDiff(center1 - center0, (Vector2)normal)) > Math.PI / 2) { contact.Manifold.LocalNormal *= -1; } } if (!IsContactValid(contact)) { contact.Enabled = false; //return; } else { Actor actor0 = BodyExt.GetData(contact.FixtureA.Body).Actor; Actor actor1 = BodyExt.GetData(contact.FixtureB.Body).Actor; actor0.CallOnCollision(actor1, true); actor1.CallOnCollision(actor0, false); } #region Debug if (DebugMode) { FixtureData[] userData = new FixtureData[2]; userData[0] = FixtureExt.GetData(contact.FixtureA); userData[1] = FixtureExt.GetData(contact.FixtureB); Xna.Vector2 normal; FixedArray2<Xna.Vector2> vList; contact.GetWorldManifold(out normal, out vList); if (contact.Manifold.PointCount == 2) { Model line = ModelFactory.CreateLines( new LineF[] { new LineF(vList[0], vList[1]) }); if (contact.Enabled) { line.SetColor(new Vector3(1, 1, 0.2f)); } else { line.SetColor(new Vector3(0.5f, 0.5f, 0)); } line.Transform.Position += new Vector3(0, 0, 5); DebugEntity.AddModel(line); } for (int i = 0; i < 2; i++) { float scale = 0.8f; Vector3 pos = new Vector3(vList[i].X, vList[i].Y, 5); //Ignore contact points that are exactly on the origin. These are almost certainly null values. if (pos.X == 0 && pos.Y == 0) { continue; } Vector2 arrowNormal = (Vector2)normal * 0.2f * scale; if (i == 0) { arrowNormal *= -1; } Model arrow = ModelFactory.CreateArrow(pos, arrowNormal, 0.02f * scale, 0.05f * scale, 0.03f * scale); DebugEntity.AddModel(arrow); Model model = ModelFactory.CreateCube(); model.Transform.Scale = new Vector3(0.08f, 0.08f, 0.08f) * scale; DebugEntity.AddModel(model); if (contact.Enabled) { model.SetColor(new Vector3(1, 1, 0.2f)); arrow.SetColor(new Vector3(1, 1, 0.2f)); } else { model.SetColor(new Vector3(0.5f, 0.5f, 0)); arrow.SetColor(new Vector3(0.5f, 0.5f, 0)); } if (userData[0].IsPortalParentless() && userData[1].IsPortalParentless()) { arrow.SetColor(new Vector3(0.7f, 0.5f, 0.2f)); } model.Transform.Position = pos; if (!userData[i].IsPortalParentless()) { IList<Vector2> vertices = Vector2Ext.ToOtk(((PolygonShape)userData[i].Fixture.Shape).Vertices); Model fixtureModel = ModelFactory.CreatePolygon(vertices); fixtureModel.SetColor(new Vector3(0, 1, 1)); fixtureModel.Transform = userData[i].Actor.GetTransform().Get3D(); fixtureModel.Transform.Position += new Vector3(0, 0, 5); fixtureModel.Transform.Scale = Vector3.One; DebugEntity.AddModel(fixtureModel); } } } #endregion } }
public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2, ref Manifold manifold1, ref Manifold manifold2) { state1 = new FixedArray2<PointState>(); state2 = new FixedArray2<PointState>(); // Detect persists and removes. for (int i = 0; i < manifold1.PointCount; ++i) { ContactID id = manifold1.Points[i].Id; state1[i] = PointState.Remove; for (int j = 0; j < manifold2.PointCount; ++j) { if (manifold2.Points[j].Id.Key == id.Key) { state1[i] = PointState.Persist; break; } } } // Detect persists and adds. for (int i = 0; i < manifold2.PointCount; ++i) { ContactID id = manifold2.Points[i].Id; state2[i] = PointState.Add; for (int j = 0; j < manifold1.PointCount; ++j) { if (manifold1.Points[j].Id.Key == id.Key) { state2[i] = PointState.Persist; break; } } } }
/// <summary> /// Get the contact manifold. Do not modify the manifold unless you understand the /// internals of Box2D. /// </summary> /// <param name="manifold">The manifold.</param> public void GetManifold(out Manifold manifold) { manifold = Manifold; }
/// <summary> /// Compute the collision manifold between a polygon and a circle. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polygonA">The polygon A.</param> /// <param name="transformA">The transform of A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform of B.</param> public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = new Vector2( transformB.Position.X + transformB.R.Col1.X * circleB.Position.X + transformB.R.Col2.X * circleB.Position.Y, transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X + transformB.R.Col2.Y * circleB.Position.Y); Vector2 cLocal = new Vector2( (c.X - transformA.Position.X) * transformA.R.Col1.X + (c.Y - transformA.Position.Y) * transformA.R.Col1.Y, (c.X - transformA.Position.X) * transformA.R.Col2.X + (c.Y - transformA.Position.Y) * transformA.R.Col2.Y); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.MaxFloat; float radius = polygonA.Radius + circleB.Radius; int vertexCount = polygonA.Vertices.Count; for (int i = 0; i < vertexCount; ++i) { Vector2 value1 = polygonA.Normals[i]; Vector2 value2 = cLocal - polygonA.Vertices[i]; float s = value1.X * value2.X + value1.Y * value2.Y; 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; Vector2 v1 = polygonA.Vertices[vertIndex1]; Vector2 v2 = polygonA.Vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.Epsilon) { manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[normalIndex]; manifold.LocalPoint = 0.5f * (v1 + v2); ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; return; } // Compute barycentric coordinates float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y); float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y); if (u1 <= 0.0f) { float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y); if (r > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v1; float factor = 1f / (float) Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + manifold.LocalNormal.Y * manifold.LocalNormal.Y); manifold.LocalNormal.X = manifold.LocalNormal.X * factor; manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; manifold.LocalPoint = v1; ManifoldPoint p0b = manifold.Points[0]; p0b.LocalPoint = circleB.Position; p0b.Id.Key = 0; manifold.Points[0] = p0b; } else if (u2 <= 0.0f) { float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y); if (r > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v2; float factor = 1f / (float) Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + manifold.LocalNormal.Y * manifold.LocalNormal.Y); manifold.LocalNormal.X = manifold.LocalNormal.X * factor; manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; manifold.LocalPoint = v2; ManifoldPoint p0c = manifold.Points[0]; p0c.LocalPoint = circleB.Position; p0c.Id.Key = 0; manifold.Points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); Vector2 value1 = cLocal - faceCenter; Vector2 value2 = polygonA.Normals[vertIndex1]; float separation2 = value1.X * value2.X + value1.Y * value2.Y; if (separation2 > radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[vertIndex1]; manifold.LocalPoint = faceCenter; ManifoldPoint p0d = manifold.Points[0]; p0d.LocalPoint = circleB.Position; p0d.Id.Key = 0; manifold.Points[0] = p0d; } }
/// <summary> /// Evaluate this contact with your own manifold and transforms. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The first transform.</param> /// <param name="transformB">The second transform.</param> private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB) { switch (_type) { case ContactType.Polygon: Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.PolygonAndCircle: Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.EdgeAndCircle: Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.EdgeAndPolygon: Collision.Collision.CollideEdgeAndPolygon(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.ChainAndCircle: ChainShape chain = (ChainShape)FixtureA.Shape; chain.GetChildEdge(ref _edge, ChildIndexA); Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.ChainAndPolygon: ChainShape loop2 = (ChainShape)FixtureA.Shape; loop2.GetChildEdge(ref _edge, ChildIndexA); Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.Circle: Collision.Collision.CollideCircles(ref manifold, (CircleShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; } }
/// <summary> /// Evaluate this contact with your own manifold and transforms. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The first transform.</param> /// <param name="transformB">The second transform.</param> void evaluate( ref Manifold manifold, ref Transform transformA, ref Transform transformB ) { switch( _type ) { case ContactType.Polygon: Collision.Collision.collidePolygons( ref manifold, (PolygonShape)fixtureA.shape, ref transformA, (PolygonShape)fixtureB.shape, ref transformB ); break; case ContactType.PolygonAndCircle: Collision.Collision.collidePolygonAndCircle( ref manifold, (PolygonShape)fixtureA.shape, ref transformA, (CircleShape)fixtureB.shape, ref transformB ); break; case ContactType.EdgeAndCircle: Collision.Collision.collideEdgeAndCircle( ref manifold, (EdgeShape)fixtureA.shape, ref transformA, (CircleShape)fixtureB.shape, ref transformB ); break; case ContactType.EdgeAndPolygon: Collision.Collision.collideEdgeAndPolygon( ref manifold, (EdgeShape)fixtureA.shape, ref transformA, (PolygonShape)fixtureB.shape, ref transformB ); break; case ContactType.ChainAndCircle: var chain = (ChainShape)fixtureA.shape; chain.getChildEdge( _edge, childIndexA ); Collision.Collision.collideEdgeAndCircle( ref manifold, _edge, ref transformA, (CircleShape)fixtureB.shape, ref transformB ); break; case ContactType.ChainAndPolygon: var loop2 = (ChainShape)fixtureA.shape; loop2.getChildEdge( _edge, childIndexA ); Collision.Collision.collideEdgeAndPolygon( ref manifold, _edge, ref transformA, (PolygonShape)fixtureB.shape, ref transformB ); break; case ContactType.Circle: Collision.Collision.collideCircles( ref manifold, (CircleShape)fixtureA.shape, ref transformA, (CircleShape)fixtureB.shape, ref transformB ); break; } }
/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="transformA">The transform A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform B.</param> public static void collideEdgeAndCircle( ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB ) { manifold.pointCount = 0; // Compute circle in frame of edge var Q = MathUtils.mulT( ref transformA, MathUtils.Mul( ref transformB, ref circleB._position ) ); Vector2 A = edgeA.vertex1, B = edgeA.vertex2; var e = B - A; // Barycentric coordinates var u = Vector2.Dot( e, B - Q ); var v = Vector2.Dot( e, Q - A ); var radius = edgeA.radius + circleB.radius; ContactFeature cf; cf.indexB = 0; cf.typeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if( v <= 0.0f ) { P = A; d = Q - P; float dd; Vector2.Dot( ref d, ref d, out dd ); if( dd > radius * radius ) return; // Is there an edge connected to A? if( edgeA.hasVertex0 ) { var A1 = edgeA.vertex0; var B1 = A; var e1 = B1 - A1; var u1 = Vector2.Dot( e1, B1 - Q ); // Is the circle in Region AB of the previous edge? if( u1 > 0.0f ) return; } cf.indexA = 0; cf.typeA = (byte)ContactFeatureType.Vertex; manifold.pointCount = 1; manifold.type = ManifoldType.Circles; manifold.localNormal = Vector2.Zero; manifold.localPoint = P; var mp = new ManifoldPoint(); mp.id.key = 0; mp.id.features = cf; mp.localPoint = circleB.position; manifold.points[0] = mp; return; } // Region B if( u <= 0.0f ) { P = B; d = Q - P; float dd; Vector2.Dot( ref d, ref d, out dd ); if( dd > radius * radius ) return; // Is there an edge connected to B? if( edgeA.hasVertex3 ) { var B2 = edgeA.vertex3; var A2 = B; var e2 = B2 - A2; var v2 = Vector2.Dot( e2, Q - A2 ); // Is the circle in Region AB of the next edge? if( v2 > 0.0f ) return; } cf.indexA = 1; cf.typeA = (byte)ContactFeatureType.Vertex; manifold.pointCount = 1; manifold.type = ManifoldType.Circles; manifold.localNormal = Vector2.Zero; manifold.localPoint = P; var mp = new ManifoldPoint(); mp.id.key = 0; mp.id.features = cf; mp.localPoint = circleB.position; manifold.points[0] = mp; return; } // Region AB float den; Vector2.Dot( ref e, ref e, out den ); Debug.Assert( den > 0.0f ); P = ( 1.0f / den ) * ( u * A + v * B ); d = Q - P; float dd2; Vector2.Dot( ref d, ref d, out dd2 ); if( dd2 > radius * radius ) return; var n = new Vector2( -e.Y, e.X ); if( Vector2.Dot( n, Q - A ) < 0.0f ) n = new Vector2( -n.X, -n.Y ); n.Normalize(); cf.indexA = 0; cf.typeA = (byte)ContactFeatureType.Face; manifold.pointCount = 1; manifold.type = ManifoldType.FaceA; manifold.localNormal = n; manifold.localPoint = A; ManifoldPoint mp2 = new ManifoldPoint(); mp2.id.key = 0; mp2.id.features = cf; mp2.localPoint = circleB.position; manifold.points[0] = mp2; }