public Entity BuildEntity(Entity e, params object[] args) { e.Tag = "Chassis"; #region Body Body Chassis = e.AddComponent<Body>(new Body(_World, e)); { Vertices vertices = new Vertices(8); vertices.Add(new Vector2(-2.5f, 0.08f)); vertices.Add(new Vector2(-2.375f, -0.46f)); vertices.Add(new Vector2(-0.58f, -0.92f)); vertices.Add(new Vector2(0.46f, -0.92f)); vertices.Add(new Vector2(2.5f, -0.17f)); vertices.Add(new Vector2(2.5f, 0.205f)); vertices.Add(new Vector2(2.3f, 0.33f)); vertices.Add(new Vector2(-2.25f, 0.35f)); PolygonShape chassisShape = new PolygonShape(vertices, 2f); Chassis.BodyType = BodyType.Dynamic; Chassis.Position = new Vector2(0.0f, -1.0f); Chassis.CreateFixture(chassisShape); } #endregion #region Sprite e.AddComponent<Sprite>(new Sprite(args[0] as Texture2D, (Rectangle)args[1], Chassis, 1, Color.White, 0f)); #endregion return e; }
public Entity[] BuildEntityGroup(EntityWorld world, params object[] args) { const int segmentCount = 20; Entity[] segments = new Entity[segmentCount]; //Make teh bridge segments and bind dem togeder PolygonShape shape = new PolygonShape(1f); shape.SetAsBox(1.0f, 0.125f); Body prevBody = args[0] as Body; for (int i = 0; i < segmentCount; ++i) { Entity segment = world.CreateEntity(); segment.Tag = "segment" + i; Body body = segment.AddComponent<Body>(new Body(world, segment)); body.BodyType = BodyType.Dynamic; body.Position = new Vector2(161f + 2f * i, 0.125f); Fixture fix = body.CreateFixture(shape); fix.Friction = 0.6f; JointFactory.CreateRevoluteJoint(world, prevBody, body, -Vector2.UnitX); segments[i] = segment; prevBody = body; } JointFactory.CreateRevoluteJoint(world, args[0] as Body, prevBody, Vector2.UnitX); return segments; }
public PhysicsBreakableBody(IEnumerable<Vertices> vertices, PhysicsWorld world, float density, object userData) { _world = world; _world.ContactManager.PostSolve += PostSolve; MainBody = new PhysicsBody(_world); MainBody.BodyType = BodyType.Dynamic; foreach (Vertices part in vertices) { PolygonShape polygonShape = new PolygonShape(part, density); Fixture fixture = MainBody.CreateFixture(polygonShape, userData); Parts.Add(fixture); } }
public static List<Fixture> AttachCompoundPolygon(List<Vertices> list, float density, PhysicsBody body, object userData) { List<Fixture> res = new List<Fixture>(list.Count); //Then we create several fixtures using the body foreach (Vertices vertices in list) { if (vertices.Count == 2) { EdgeShape shape = new EdgeShape(vertices[0], vertices[1]); res.Add(body.CreateFixture(shape, userData)); } else { PolygonShape shape = new PolygonShape(vertices, density); res.Add(body.CreateFixture(shape, userData)); } } return res; }
/// <summary> /// Creates a chain. /// </summary> /// <param name="world">The world.</param> /// <param name="start">The start.</param> /// <param name="end">The end.</param> /// <param name="linkWidth">The width.</param> /// <param name="linkHeight">The height.</param> /// <param name="fixStart">if set to <c>true</c> [fix start].</param> /// <param name="fixEnd">if set to <c>true</c> [fix end].</param> /// <param name="numberOfLinks">The number of links.</param> /// <param name="linkDensity">The link density.</param> /// <returns></returns> public static Path CreateChain(PhysicsWorld world, Vector2 start, Vector2 end, float linkWidth, float linkHeight, bool fixStart, bool fixEnd, int numberOfLinks, float linkDensity) { //Chain start / end Path path = new Path(); path.Add(start); path.Add(end); //A single chainlink PolygonShape shape = new PolygonShape(PolygonTools.CreateRectangle(linkWidth, linkHeight), linkDensity); //Use PathManager to create all the chainlinks based on the chainlink created before. List<PhysicsBody> chainLinks = PathManager.EvenlyDistributeShapesAlongPath(world, path, shape, BodyType.Dynamic, numberOfLinks); if (fixStart) { //Fix the first chainlink to the world JointFactory.CreateFixedRevoluteJoint(world, chainLinks[0], new Vector2(0, -(linkHeight / 2)), chainLinks[0].Position); } if (fixEnd) { //Fix the last chainlink to the world JointFactory.CreateFixedRevoluteJoint(world, chainLinks[chainLinks.Count - 1], new Vector2(0, (linkHeight / 2)), chainLinks[chainLinks.Count - 1].Position); } //Attach all the chainlinks together with a revolute joint PathManager.AttachBodiesWithRevoluteJoint(world, chainLinks, new Vector2(0, -linkHeight), new Vector2(0, linkHeight), false, false); return (path); }
public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, PhysicsBody body, object userData) { if (xRadius <= 0) throw new ArgumentOutOfRangeException("xRadius", "X-radius must be more than 0"); if (yRadius <= 0) throw new ArgumentOutOfRangeException("yRadius", "Y-radius must be more than 0"); Vertices ellipseVertices = PolygonTools.CreateEllipse(xRadius, yRadius, edges); PolygonShape polygonShape = new PolygonShape(ellipseVertices, density); return body.CreateFixture(polygonShape, userData); }
public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, PhysicsBody body, object userData) { Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); rectangleVertices.Translate(ref offset); PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); return body.CreateFixture(rectangleShape, userData); }
public static Fixture AttachPolygon(Vertices vertices, float density, PhysicsBody body, object userData) { if (vertices.Count <= 1) throw new ArgumentOutOfRangeException("vertices", "Too few points to be a polygon"); PolygonShape polygon = new PolygonShape(vertices, density); return body.CreateFixture(polygon, userData); }
/// <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; } }
public bool CompareTo(PolygonShape shape) { if (Vertices.Count != shape.Vertices.Count) return false; for (int i = 0; i < Vertices.Count; i++) { if (Vertices[i] != shape.Vertices[i]) return false; } return (Radius == shape.Radius && MassData == shape.MassData); }
public static PhysicsBody CreateRectangle(PhysicsWorld world, float width, float height, float density, Vector2 position, object userData) { if (width <= 0) throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters"); if (height <= 0) throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters"); PhysicsBody newBody = CreateBody(world, position); Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); newBody.CreateFixture(rectangleShape, userData); return newBody; }
/// <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 }
/// <summary> /// Find the max separation between poly1 and poly2 using edge normals from poly1. /// </summary> /// <param name="edgeIndex">Index of the edge.</param> /// <param name="poly1">The poly1.</param> /// <param name="xf1">The XF1.</param> /// <param name="poly2">The poly2.</param> /// <param name="xf2">The XF2.</param> /// <returns></returns> private static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1, PolygonShape poly2, ref Transform xf2) { int count1 = poly1.Vertices.Count; // Vector pointing from the centroid of poly1 to the centroid of poly2. float dx = (xf2.Position.X + xf2.R.Col1.X * poly2.MassData.Centroid.X + xf2.R.Col2.X * poly2.MassData.Centroid.Y) - (xf1.Position.X + xf1.R.Col1.X * poly1.MassData.Centroid.X + xf1.R.Col2.X * poly1.MassData.Centroid.Y); float dy = (xf2.Position.Y + xf2.R.Col1.Y * poly2.MassData.Centroid.X + xf2.R.Col2.Y * poly2.MassData.Centroid.Y) - (xf1.Position.Y + xf1.R.Col1.Y * poly1.MassData.Centroid.X + xf1.R.Col2.Y * poly1.MassData.Centroid.Y); Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.X + dy * xf1.R.Col1.Y, dx * xf1.R.Col2.X + dy * xf1.R.Col2.Y); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -Settings.MaxFloat; for (int i = 0; i < count1; ++i) { float dot = Vector2.Dot(poly1.Normals[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2); // Find the best edge and the search direction. int bestEdge; float bestSeparation; int increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { edgeIndex = edge; return s; } // Perform a local search for the best edge normal. for (; ; ) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; else edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } edgeIndex = bestEdge; return bestSeparation; }
private static void FindIncidentEdge(out FixedArray2<ClipVertex> c, PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) { c = new FixedArray2<ClipVertex>(); int count2 = poly2.Vertices.Count; Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); // Get the normal of the reference edge in poly2's frame. Vector2 v = poly1.Normals[edge1]; float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y; float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y; Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y, tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y); // Find the incident edge on poly2. int index = 0; float minDot = Settings.MaxFloat; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(normal1, poly2.Normals[i]); if (dot < minDot) { minDot = dot; index = i; } } // Build the clip vertices for the incident edge. int i1 = index; int i2 = i1 + 1 < count2 ? i1 + 1 : 0; ClipVertex cv0 = c[0]; Vector2 v1 = poly2.Vertices[i1]; cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y; cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y; cv0.ID.Features.IndexA = (byte)edge1; cv0.ID.Features.IndexB = (byte)i1; cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face; cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; c[0] = cv0; ClipVertex cv1 = c[1]; Vector2 v2 = poly2.Vertices[i2]; cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y; cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y; cv1.ID.Features.IndexA = (byte)edge1; cv1.ID.Features.IndexB = (byte)i2; cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face; cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; c[1] = cv1; }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> /// <param name="poly1">The poly1.</param> /// <param name="xf1">The XF1.</param> /// <param name="edge1">The edge1.</param> /// <param name="poly2">The poly2.</param> /// <param name="xf2">The XF2.</param> /// <returns></returns> private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) { int count2 = poly2.Vertices.Count; Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); // Convert normal from poly1's frame into poly2's frame. Vector2 p1n = poly1.Normals[edge1]; float normalWorldx = xf1.R.Col1.X * p1n.X + xf1.R.Col2.X * p1n.Y; float normalWorldy = xf1.R.Col1.Y * p1n.X + xf1.R.Col2.Y * p1n.Y; Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.X + normalWorldy * xf2.R.Col1.Y, normalWorldx * xf2.R.Col2.X + normalWorldy * xf2.R.Col2.Y); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Settings.MaxFloat; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(poly2.Vertices[i], normal); if (dot < minDot) { minDot = dot; index = i; } } Vector2 p1ve = poly1.Vertices[edge1]; Vector2 p2vi = poly2.Vertices[index]; return ((xf2.Position.X + xf2.R.Col1.X * p2vi.X + xf2.R.Col2.X * p2vi.Y) - (xf1.Position.X + xf1.R.Col1.X * p1ve.X + xf1.R.Col2.X * p1ve.Y)) * normalWorldx + ((xf2.Position.Y + xf2.R.Col1.Y * p2vi.X + xf2.R.Col2.Y * p2vi.Y) - (xf1.Position.Y + xf1.R.Col1.Y * p1ve.X + xf1.R.Col2.Y * p1ve.Y)) * normalWorldy; }
/// <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; }
public override Shape Clone() { PolygonShape clone = new PolygonShape(); clone.ShapeType = ShapeType; clone._radius = _radius; clone._density = _density; if (Settings.ConserveMemory) { clone.Vertices = Vertices; clone.Normals = Normals; } else { clone.Vertices = new Vertices(Vertices); clone.Normals = new Vertices(Normals); } clone.MassData = MassData; return clone; }
public void Deserialize(PhysicsWorld world, Stream stream) { world.Clear(); XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream); if (root.Name.ToLower() != "world") throw new Exception(); foreach (XMLFragmentElement main in root.Elements) { if (main.Name.ToLower() == "gravity") { world.Gravity = ReadVector(main); } } foreach (XMLFragmentElement shapeElement in root.Elements) { if (shapeElement.Name.ToLower() == "shapes") { foreach (XMLFragmentElement n in shapeElement.Elements) { if (n.Name.ToLower() != "shape") throw new Exception(); ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), n.Attributes[0].Value, true); switch (type) { case ShapeType.Circle: { CircleShape shape = new CircleShape(); foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "radius": shape.Radius = float.Parse(sn.Value); break; case "position": shape.Position = ReadVector(sn); break; default: throw new Exception(); } } _shapes.Add(shape); } break; case ShapeType.Polygon: { PolygonShape shape = new PolygonShape(); foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "vertices": { List<Vector2> verts = new List<Vector2>(); foreach (XMLFragmentElement vert in sn.Elements) verts.Add(ReadVector(vert)); shape.Set(new Vertices(verts.ToArray())); } break; case "centroid": shape.MassData.Centroid = ReadVector(sn); break; } } _shapes.Add(shape); } break; case ShapeType.Edge: { EdgeShape shape = new EdgeShape(); foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "hasvertex0": shape.HasVertex0 = bool.Parse(sn.Value); break; case "hasvertex3": shape.HasVertex0 = bool.Parse(sn.Value); break; case "vertex0": shape.Vertex0 = ReadVector(sn); break; case "vertex1": shape.Vertex1 = ReadVector(sn); break; case "vertex2": shape.Vertex2 = ReadVector(sn); break; case "vertex3": shape.Vertex3 = ReadVector(sn); break; default: throw new Exception(); } } _shapes.Add(shape); } break; } } } } foreach (XMLFragmentElement fixtureElement in root.Elements) { if (fixtureElement.Name.ToLower() == "fixtures") { foreach (XMLFragmentElement n in fixtureElement.Elements) { Fixture fixture = new Fixture(); if (n.Name.ToLower() != "fixture") throw new Exception(); foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "shape": fixture.Shape = _shapes[int.Parse(sn.Value)]; break; case "density": fixture.Shape.Density = float.Parse(sn.Value); break; case "filterdata": foreach (XMLFragmentElement ssn in sn.Elements) { switch (ssn.Name.ToLower()) { case "categorybits": fixture._collisionCategories = (Category)int.Parse(ssn.Value); break; case "maskbits": fixture._collidesWith = (Category)int.Parse(ssn.Value); break; case "groupindex": fixture._collisionGroup = short.Parse(ssn.Value); break; } } break; case "friction": fixture.Friction = float.Parse(sn.Value); break; case "issensor": fixture.IsSensor = bool.Parse(sn.Value); break; case "restitution": fixture.Restitution = float.Parse(sn.Value); break; case "userdata": fixture.UserData = ReadSimpleType(sn, null, false); break; } } _fixtures.Add(fixture); } } } foreach (XMLFragmentElement bodyElement in root.Elements) { if (bodyElement.Name.ToLower() == "bodies") { foreach (XMLFragmentElement n in bodyElement.Elements) { PhysicsBody body = new PhysicsBody(world); if (n.Name.ToLower() != "body") throw new Exception(); body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), n.Attributes[0].Value, true); foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "active": if (bool.Parse(sn.Value)) body.Flags |= BodyFlags.Enabled; else body.Flags &= ~BodyFlags.Enabled; break; case "allowsleep": body.SleepingAllowed = bool.Parse(sn.Value); break; case "angle": { Vector2 position = body.Position; body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value)); } break; case "angulardamping": body.AngularDamping = float.Parse(sn.Value); break; case "angularvelocity": body.AngularVelocity = float.Parse(sn.Value); break; case "awake": body.Awake = bool.Parse(sn.Value); break; case "bullet": body.IsBullet = bool.Parse(sn.Value); break; case "fixedrotation": body.FixedRotation = bool.Parse(sn.Value); break; case "lineardamping": body.LinearDamping = float.Parse(sn.Value); break; case "linearvelocity": body.LinearVelocity = ReadVector(sn); break; case "position": { float rotation = body.Rotation; Vector2 position = ReadVector(sn); body.SetTransformIgnoreContacts(ref position, rotation); } break; case "userdata": body.UserData = ReadSimpleType(sn, null, false); break; case "fixtures": { foreach (XMLFragmentElement v in sn.Elements) { Fixture blueprint = _fixtures[int.Parse(v.Value)]; Fixture f = new Fixture(body, blueprint.Shape); f.Restitution = blueprint.Restitution; f.UserData = blueprint.UserData; f.Friction = blueprint.Friction; f.CollidesWith = blueprint.CollidesWith; f.CollisionCategories = blueprint.CollisionCategories; f.CollisionGroup = blueprint.CollisionGroup; } break; } } } _bodies.Add(body); } } } foreach (XMLFragmentElement jointElement in root.Elements) { if (jointElement.Name.ToLower() == "joints") { foreach (XMLFragmentElement n in jointElement.Elements) { PhysicsJoint joint; if (n.Name.ToLower() != "joint") throw new Exception(); JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true); int bodyAIndex = -1, bodyBIndex = -1; bool collideConnected = false; object userData = null; foreach (XMLFragmentElement sn in n.Elements) { switch (sn.Name.ToLower()) { case "bodya": bodyAIndex = int.Parse(sn.Value); break; case "bodyb": bodyBIndex = int.Parse(sn.Value); break; case "collideconnected": collideConnected = bool.Parse(sn.Value); break; case "userdata": userData = ReadSimpleType(sn, null, false); break; } } PhysicsBody bodyA = _bodies[bodyAIndex]; PhysicsBody bodyB = _bodies[bodyBIndex]; switch (type) { case JointType.Distance: joint = new DistanceJoint(); break; case JointType.Friction: joint = new FrictionJoint(); break; case JointType.Line: joint = new LineJoint(); break; case JointType.Prismatic: joint = new PrismaticJoint(); break; case JointType.Pulley: joint = new PulleyJoint(); break; case JointType.Revolute: joint = new RevoluteJoint(); break; case JointType.Weld: joint = new WeldJoint(); break; case JointType.Rope: joint = new RopeJoint(); break; case JointType.Angle: joint = new AngleJoint(); break; case JointType.Slider: joint = new SliderJoint(); break; case JointType.Gear: throw new Exception("GearJoint is not supported."); default: throw new Exception("Invalid or unsupported joint."); } joint.CollideConnected = collideConnected; joint.UserData = userData; joint.BodyA = bodyA; joint.BodyB = bodyB; _joints.Add(joint); world.AddJoint(joint); foreach (XMLFragmentElement sn in n.Elements) { // check for specific nodes switch (type) { case JointType.Distance: { switch (sn.Name.ToLower()) { case "dampingratio": ((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value); break; case "frequencyhz": ((DistanceJoint)joint).Frequency = float.Parse(sn.Value); break; case "length": ((DistanceJoint)joint).Length = float.Parse(sn.Value); break; case "localanchora": ((DistanceJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((DistanceJoint)joint).LocalAnchorB = ReadVector(sn); break; } } break; case JointType.Friction: { switch (sn.Name.ToLower()) { case "localanchora": ((FrictionJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((FrictionJoint)joint).LocalAnchorB = ReadVector(sn); break; case "maxforce": ((FrictionJoint)joint).MaxForce = float.Parse(sn.Value); break; case "maxtorque": ((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value); break; } } break; case JointType.Line: { switch (sn.Name.ToLower()) { case "enablemotor": ((LineJoint)joint).MotorEnabled = bool.Parse(sn.Value); break; case "localanchora": ((LineJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((LineJoint)joint).LocalAnchorB = ReadVector(sn); break; case "motorspeed": ((LineJoint)joint).MotorSpeed = float.Parse(sn.Value); break; case "dampingratio": ((LineJoint)joint).DampingRatio = float.Parse(sn.Value); break; case "maxmotortorque": ((LineJoint)joint).MaxMotorTorque = float.Parse(sn.Value); break; case "frequencyhz": ((LineJoint)joint).Frequency = float.Parse(sn.Value); break; case "localxaxis": ((LineJoint)joint).LocalXAxis = ReadVector(sn); break; } } break; case JointType.Prismatic: { switch (sn.Name.ToLower()) { case "enablelimit": ((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value); break; case "enablemotor": ((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value); break; case "localanchora": ((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn); break; case "local1axis1": ((PrismaticJoint)joint).LocalXAxis1 = ReadVector(sn); break; case "maxmotorforce": ((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value); break; case "motorspeed": ((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value); break; case "lowertranslation": ((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value); break; case "uppertranslation": ((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value); break; case "referenceangle": ((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value); break; } } break; case JointType.Pulley: { switch (sn.Name.ToLower()) { case "groundanchora": ((PulleyJoint)joint).GroundAnchorA = ReadVector(sn); break; case "groundanchorb": ((PulleyJoint)joint).GroundAnchorB = ReadVector(sn); break; case "lengtha": ((PulleyJoint)joint).LengthA = float.Parse(sn.Value); break; case "lengthb": ((PulleyJoint)joint).LengthB = float.Parse(sn.Value); break; case "localanchora": ((PulleyJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((PulleyJoint)joint).LocalAnchorB = ReadVector(sn); break; case "maxlengtha": ((PulleyJoint)joint).MaxLengthA = float.Parse(sn.Value); break; case "maxlengthb": ((PulleyJoint)joint).MaxLengthB = float.Parse(sn.Value); break; case "ratio": ((PulleyJoint)joint).Ratio = float.Parse(sn.Value); break; } } break; case JointType.Revolute: { switch (sn.Name.ToLower()) { case "enablelimit": ((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value); break; case "enablemotor": ((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value); break; case "localanchora": ((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn); break; case "maxmotortorque": ((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value); break; case "motorspeed": ((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value); break; case "lowerangle": ((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value); break; case "upperangle": ((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value); break; case "referenceangle": ((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value); break; } } break; case JointType.Weld: { switch (sn.Name.ToLower()) { case "localanchora": ((WeldJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((WeldJoint)joint).LocalAnchorB = ReadVector(sn); break; } } break; case JointType.Rope: { switch (sn.Name.ToLower()) { case "localanchora": ((RopeJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((RopeJoint)joint).LocalAnchorB = ReadVector(sn); break; case "maxlength": ((RopeJoint)joint).MaxLength = float.Parse(sn.Value); break; } } break; case JointType.Gear: throw new Exception("Gear joint is unsupported"); case JointType.Angle: { switch (sn.Name.ToLower()) { case "biasfactor": ((AngleJoint)joint).BiasFactor = float.Parse(sn.Value); break; case "maximpulse": ((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value); break; case "softness": ((AngleJoint)joint).Softness = float.Parse(sn.Value); break; case "targetangle": ((AngleJoint)joint).TargetAngle = float.Parse(sn.Value); break; } } break; case JointType.Slider: { switch (sn.Name.ToLower()) { case "dampingratio": ((SliderJoint)joint).DampingRatio = float.Parse(sn.Value); break; case "frequencyhz": ((SliderJoint)joint).Frequency = float.Parse(sn.Value); break; case "maxlength": ((SliderJoint)joint).MaxLength = float.Parse(sn.Value); break; case "minlength": ((SliderJoint)joint).MinLength = float.Parse(sn.Value); break; case "localanchora": ((SliderJoint)joint).LocalAnchorA = ReadVector(sn); break; case "localanchorb": ((SliderJoint)joint).LocalAnchorB = ReadVector(sn); break; } } break; } } } } } }