public override void evaluate(Manifold manifold, Transform xfA, Transform xfB) { ChainShape chain = (ChainShape) m_fixtureA.getShape(); chain.getChildEdge(edge, m_indexA); pool.getCollision().collideEdgeAndPolygon(manifold, edge, xfA, (PolygonShape) m_fixtureB.getShape(), xfB); }
public DrawingEditDetail(Manifold.Interop.Point p_objPoint) { this.X = p_objPoint.X; this.Y = p_objPoint.Y; this.XCoord = p_objPoint.X.ToString(); this.YCoord = p_objPoint.Y.ToString(); }
public override void Step(Framework.Settings settings) { Manifold manifold = new Manifold(); Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB); WorldManifold worldManifold = new WorldManifold(ref manifold, ref _transformA, _polygonA._radius, ref _transformB, _polygonB._radius); _debugDraw.DrawString(50, _textLine, "point count = {0:n}", manifold._pointCount); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); FixedArray8<Vector2> v = new FixedArray8<Vector2>(); for (int i = 0; i < _polygonA._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color); for (int i = 0; i < _polygonB._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color); } for (int i = 0; i < manifold._pointCount; ++i) { _debugDraw.DrawPoint(worldManifold._points[i], 0.5f, new Color(0.9f, 0.3f, 0.3f)); } }
public void OnCollide(Manifold m) { //Find which collider is another node. Node otherNode; //TODO:ASTEROIDS COLLIDE WITH THEIR OWN GHOSTS! if (m.A == Collision || m.B == Collision) //If we are one of the collision pairings { otherNode = m.A != Collision ? m.A : m.B; //Find which one is the other node. } else { //TODO: Asteroids do not resolve with ghosts. //Oncollide was called from a ghost, find which one isn't the ghost otherNode = m.A.Parent.GetType() == typeof(AsteroidGhost) ? m.A : m.B; Asteroid a = (otherNode.Parent as AsteroidGhost).GetOriginal(); //Use a special resolver to resove the collision Manifold specialresolver = new Manifold(Collision, a.Collision); CollisionHandler.ResolveCollision(specialresolver); CollisionHandler.PositionalCorrection(specialresolver); } if (otherNode.Parent.GetType() == typeof(Bullet) || otherNode.Parent.GetType() == typeof(PlayerShip)) { Health.Hurt(1); } }
public static bool CirclevsCircle(Manifold M) { // Setup manifold object Circle A = (Circle)((Ball)M.m_A).m_hitbox; Circle B = (Circle)((Ball)M.m_B).m_hitbox; // Vector from A to B Vector2 n = B.m_position - A.m_position; float r = A.m_radius + B.m_radius; r *= r; if(n.LengthSquared() > r) return false; // Compute manifold float d = n.Length(); if(d != 0) { M.m_penetration = r - d; M.m_normal = n / d; } else { M.m_penetration = A.m_radius; M.m_normal = new Vector2(1, 0); } return true; }
public void ConnectEvents(Manifold.Interop.Scripts.Events ev) { ev.DocumentClosed += new Manifold.Interop.Scripts.Events.DocumentClosedEventHandler(DocumentChanged); ev.DocumentCreated += new Manifold.Interop.Scripts.Events.DocumentCreatedEventHandler(DocumentChanged); ev.DocumentOpened += new Manifold.Interop.Scripts.Events.DocumentOpenedEventHandler(DocumentChanged); ev.DocumentSaved += new Manifold.Interop.Scripts.Events.DocumentSavedEventHandler(DocumentChanged); ev.AddinLoaded += new Manifold.Interop.Scripts.Events.AddinLoadedEventHandler(DocumentChanged); }
//NOTE: I think the axManifoldMap.NativeToScreen() method does the same as this. public static Manifold.Interop.Point convertPointCoordinates(AxManifold.Interop.AxComponentControl p_Target, Manifold.Interop.Drawing p_Source, Manifold.Interop.Point p_objPoint) { Manifold.Interop.CoordinateSystem csSource = null; Manifold.Interop.CoordinateSystem csTarget = null; csTarget = p_Target.Application.DefaultCoordinateSystemLatLon; if (p_Source.CoordinateSystemVerified == true) { csSource = p_Source.CoordinateSystem; } else { p_Source.CoordinateSystem = p_Target.Application.Application.NewCoordinateSystem("Irish Grid"); csSource = p_Target.Application.Application.NewCoordinateSystem("Irish Grid"); p_Source.CoordinateSystemVerified = true; } Manifold.Interop.CoordinateConverter objConverter = p_Target.Application.NewCoordinateConverter(); objConverter.Prepare((Manifold.Interop.Base)csTarget, (Manifold.Interop.Base)csSource); objConverter.Convert((Manifold.Interop.Base)p_objPoint, null); return p_objPoint; }
public static void DrawArea(Manifold.Interop.Drawing p_Drawing, Manifold.Interop.Geom p_objGeomArea) { try { Manifold.Interop.Table objTable = (Manifold.Interop.Table)p_Drawing.OwnedTable; StringBuilder sbSQL = new StringBuilder(); sbSQL.Append("INSERT INTO [" + objTable.Name + "] ([Geom (I)]) VALUES (AssignCoordSys(CGeom(CGeomWKB(\"" + p_objGeomArea.ToTextWKT() + "\")),CoordSys(\"" + objTable.Name + "\" as COMPONENT)));"); Manifold.Interop.Query qryQuery = p_Drawing.Document.NewQuery("Temp_AddLine", false); qryQuery.Text = sbSQL.ToString(); //Commit query qryQuery.RunEx(false); //Remove the query p_Drawing.Document.ComponentSet.Remove(qryQuery); } catch (Exception objEx) { throw; } }
/// <summary> /// Adds a coordinate to the selected Edit object /// </summary> private void addCoordinate(Manifold.Interop.Point p_objPointScreen) { Manifold.Interop.Query query = this._MapControl.get_Document().NewQuery("TempQuery", false); bool bAddPointAtEnd = false; this._AddingCoordinate = true; try { Manifold.Interop.Drawing tempDrawing = null; bool bAdditionOccured = false; if (this._DrawingOriginalEditObject.Selection.Count > 0) { StringBuilder sbSQL = new StringBuilder(); double SearchRange = this._SelectionTolerance; ; double AddCoordinateSelectionTolerance = this._SelectionToleranceAddCoordinate; //Compose a rectangular area around the clicked point - then gets its Geom WKT for use later. This will // be used to in selection later string wkt = Utility.getBoundingBoxWKT(this._DrawingEdit, p_objPointScreen, SearchRange); //Create or use existing Temp Drawing which will hold the Area\Line object that we are adding // the new Point to. The Area\Line object is Exploded into its separate Line segments int iExists = this._MapControl.get_Document().ComponentSet.ItemByName("TempDrawing"); if (iExists > 0) { tempDrawing = (Manifold.Interop.Drawing)this._MapControl.get_Document().ComponentSet["TempDrawing"]; tempDrawing.Clear(false); } else { //Create a temp Drawing to hold a copy of the object which will be Exploded into its separate Line segments tempDrawing = this._MapControl.get_Document().NewDrawing("TempDrawing", this._MapControl.Application.NewCoordinateSystem("Irish Grid"), false); } StringBuilder sbSQLExplode = new StringBuilder(); sbSQLExplode.AppendLine("INSERT INTO [" + tempDrawing.Name + "] "); sbSQLExplode.AppendLine("([Geom (I)])"); sbSQLExplode.AppendLine("SELECT [Segment] FROM [" + this._DrawingOriginalEditObject.Name + "] WHERE ID = " + this._SelectedManifoldID); sbSQLExplode.AppendLine("SPLIT BY Branches(IntersectLine(ConvertToLine([Geom (I)]), [Geom (I)])) AS [Segment]"); Utility.executeSQL(sbSQLExplode.ToString()); ////=================================================================== //// Here I attempted to explode and select in 1 SQL statement. ////=================================================================== //StringBuilder ExplodeSQL = new StringBuilder(); //ExplodeSQL.AppendLine(" "); //ExplodeSQL.AppendLine(" SELECT [Segment] FROM [" + this._DrawingOriginalEditObject.Name + "] AS [tblExploded] WHERE ID = " + this._SelectedManifoldID); //ExplodeSQL.AppendLine(" SPLIT BY Branches(IntersectLine(ConvertToLine([Geom (I)]), [Geom (I)])) AS [Segment]"); //sbSQL.Length = 0; //sbSQL.AppendLine("SELECT [BranchIndex], [PointIndex], [BranchMaxIndex] FROM [" + this._DrawingEdit.Name + "] "); //sbSQL.AppendLine("Where TOUCHES([ID],BUFFER( "); //sbSQL.AppendLine("Select StartPoint(Geom(ID)) as s FROM (" + ExplodeSQL.ToString() + ") "); //sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); //sbSQL.AppendLine("OR "); //sbSQL.AppendLine("TOUCHES([ID],BUFFER( "); //sbSQL.AppendLine("Select EndPoint(Geom(ID)) as s FROM (" + ExplodeSQL.ToString() + ") "); //sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); //sbSQL.AppendLine("ORDER BY [PointIndex] ASC "); ////=================================================== ////This working SQL uses a OR operator - would also be possible using UNION operator. //Using the bounding box WKT and the Exploded object held in the TempDrawing find where the new point should be added. //QUESTION: It should be possible to do away with the 'tempDrawing' and instead combine the above Exploded SQL and the // SQL below into 1 query. sbSQL.Length = 0; sbSQL.AppendLine("SELECT [BranchIndex], [PointIndex], [BranchMaxIndex] FROM [" + this._DrawingEdit.Name + "] "); sbSQL.AppendLine("Where TOUCHES([ID],BUFFER( "); sbSQL.AppendLine("Select StartPoint(Geom(ID)) as s FROM [" + tempDrawing.Name + "] "); sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); sbSQL.AppendLine("OR "); sbSQL.AppendLine("TOUCHES([ID],BUFFER( "); sbSQL.AppendLine("Select EndPoint(Geom(ID)) as s FROM [" + tempDrawing.Name + "] "); sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); sbSQL.AppendLine("ORDER BY [PointIndex] ASC "); query.Text = sbSQL.ToString(); query.Run(); Manifold.Interop.Table table = query.Table; Int32 iBranchIndex = 0; Int32 iFirstPointIndex = 0; Int32 iLastPointIndex; Int32 iBranchMaxIndex; if (table.RecordSet.Count > 0) { iBranchIndex = Convert.ToInt32(table.RecordSet[0].get_Data("BranchIndex")); iBranchMaxIndex = Convert.ToInt32(table.RecordSet[0].get_Data("BranchMaxIndex")); iFirstPointIndex = Convert.ToInt32(table.RecordSet[0].get_Data("PointIndex")); iLastPointIndex = Convert.ToInt32(table.RecordSet[1].get_Data("PointIndex")); if (iFirstPointIndex != iLastPointIndex) { Manifold.Interop.Branch objBranch = VBManifoldWrapper.ManifoldObjectWrapper.getBranch(this._OriginalObjectBranchSet, iBranchIndex); Manifold.Interop.PointSet objPointSetBefore = objBranch.get_PointSet(); Manifold.Interop.PointSet objPointSetAfter = this._MapControl.get_Document().Application.NewPointSet(); //Remove Query and tempDrawing tempDrawing.Document.ComponentSet.Remove("tempDrawing"); this._DrawingOriginalEditObject.Document.ComponentSet.Remove(query); //When adding point to end of GEOM i.e.- FirstIndex=0 and LastIndex=PointSet.Count // this dosnt work so dont add any points - this will bve done later. if (iFirstPointIndex == 0 && iLastPointIndex == objPointSetBefore.Count - 1) { bAddPointAtEnd = true; } else { //Determine where to insert the point for (int iIndex = 0; iIndex < objPointSetBefore.Count; iIndex++) { if (bAdditionOccured == false) { if (iIndex > iFirstPointIndex) { objPointSetAfter.Add(p_objPointScreen); bAdditionOccured = true; } } objPointSetAfter.Add(VBManifoldWrapper.ManifoldObjectWrapper.getPointFromPointSet(objPointSetBefore, iIndex)); } } //Redraw the new Object with added Point if required if (bAdditionOccured == true) { objBranch.set_PointSet(objPointSetAfter); prepareEditObject(); } else { if (bAddPointAtEnd == true) { objPointSetBefore.Add(p_objPointScreen); objBranch.set_PointSet(objPointSetBefore); prepareEditObject(); } } } } //Unset any Edit Drawing selection this._DrawingEdit.SelectNone(); this._UserAddedCoordinate = true; } } catch (Exception objEx) { throw; } finally { this._AddingCoordinate = false; } }
private void mapObjectSelection(Manifold.Interop.Point p_PointLocationScreen) { try { //Clear any Previous selection p_PointLocationScreen = axComponentControl1.ScreenToNative(p_PointLocationScreen); //Utility.unsetSelection(axComponentControl1,DrawingOriginal); //Utility.setNearestSelection(axComponentControl1, DrawingOriginal, p_PointLocationScreen, 10); Utility.setSelection(axComponentControl1, DrawingOriginal, p_PointLocationScreen, 10); axComponentControl1.Refresh(); } catch (Exception err) { MessageBox.Show("ERROR:" + err.Message); } }
public override void Evaluate(ContactListener listener) { Body body = this._shape1.GetBody(); Body body2 = this._shape2.GetBody(); Manifold manifold = this._manifold.Clone(); Collision.Collision.CollidePolygons(ref this._manifold, (PolygonShape)this._shape1, body.GetXForm(), (PolygonShape)this._shape2, body2.GetXForm()); bool[] array = new bool[2]; bool[] array2 = array; ContactPoint contactPoint = new ContactPoint(); contactPoint.Shape1 = this._shape1; contactPoint.Shape2 = this._shape2; contactPoint.Friction = Settings.MixFriction(this._shape1.Friction, this._shape2.Friction); contactPoint.Restitution = Settings.MixRestitution(this._shape1.Restitution, this._shape2.Restitution); if (this._manifold.PointCount > 0) { for (int i = 0; i < this._manifold.PointCount; i++) { ManifoldPoint manifoldPoint = this._manifold.Points[i]; manifoldPoint.NormalImpulse = 0f; manifoldPoint.TangentImpulse = 0f; bool flag = false; ContactID iD = manifoldPoint.ID; for (int j = 0; j < manifold.PointCount; j++) { if (!array2[j]) { ManifoldPoint manifoldPoint2 = manifold.Points[j]; if (manifoldPoint2.ID.Key == iD.Key) { array2[j] = true; manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse; manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse; flag = true; if (listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Persist(contactPoint); } break; } } } if (!flag && listener != null) { contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = this._manifold.Normal; contactPoint.Separation = manifoldPoint.Separation; contactPoint.ID = iD; listener.Add(contactPoint); } } this._manifoldCount = 1; } else { this._manifoldCount = 0; } if (listener != null) { for (int i = 0; i < manifold.PointCount; i++) { if (!array2[i]) { ManifoldPoint manifoldPoint2 = manifold.Points[i]; contactPoint.Position = body.GetWorldPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint1); Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint2); contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint; contactPoint.Normal = manifold.Normal; contactPoint.Separation = manifoldPoint2.Separation; contactPoint.ID = manifoldPoint2.ID; listener.Remove(contactPoint); } } } }
/// <summary> /// Add contacts for penetrating vertices. Note that this does not handle /// cases where an overlap was detected, but no vertices fall inside the /// opposing polygon (like a Star of David). For this we have a fallback. /// /// See http://chipmunk-physics.googlecode.com/svn/trunk/src/cpCollision.c /// </summary> private static void FindVerts( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { bool found = false; for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) return; found = true; } } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) return; found = true; } } // Fallback to check the degenerate "Star of David" case if (found == false) FindVertsFallback(poly1, poly2, normal, penetration, manifold); }
public static bool AABBvsCircle(Manifold M) { // Setup a couple pointers to each object Brick A = (Brick)M.m_A; Ball B = (Ball)M.m_B; // Vector from A to B Vector2 posA = new Vector2((A.m_position.X + (A.m_texture.Width / 2)), (A.m_position.Y + (A.m_texture.Height / 2))); Vector2 posB = new Vector2((B.m_position.X + (B.m_texture.Width / 2)), (B.m_position.Y + (B.m_texture.Height / 2))); Vector2 n = posB - posA; // Closest point on A to center B Vector2 closest = n; // Calculate half extents along each axis float x_extent = (((AABB)A.m_hitbox).m_max.X - ((AABB)A.m_hitbox).m_min.X) / 2; float y_extent = (((AABB)A.m_hitbox).m_max.Y - ((AABB)A.m_hitbox).m_min.Y) / 2; Vector2 extent = new Vector2(x_extent, y_extent); // Clamp point to edges of the AABB closest = Vector2.Clamp(closest, -extent, extent); bool inside = false; // Circle is inside the AABB, so we need to clamp the circle's center // to the closest edge if (n == closest) { inside = true; // Find closest axis if(Math.Abs(n.X) > Math.Abs(n.Y)) { // Clamp to closest extent X if(closest.X > 0) closest.X = extent.X; else closest.X = -extent.X; } else { // Clamp to closest extent Y if (closest.Y > 0) closest.Y = extent.Y; else closest.Y = -extent.Y; } } Vector2 normal = n - closest; float d = normal.LengthSquared(); float r = ((Circle)B.m_hitbox).m_radius; // Early out of the radius is shorter than distance to closest point and // Circle not inside the AABB if (d > (r * r) && !inside) return false; // Avoided sqrt until we need d = (float)Math.Sqrt(d); // Collision normal needs to be flipped to point outside if circle was // inside the AABB if(inside) { M.m_normal = -Vector2.Normalize(normal); M.m_penetration = r - d; } else { M.m_normal = Vector2.Normalize(normal); M.m_penetration = r - d; } return true; }
protected Contact(IWorldPool argPool) { m_fixtureA = null; m_fixtureB = null; m_nodeA = new ContactEdge(); m_nodeB = new ContactEdge(); m_manifold = new Manifold(); pool = argPool; }
/// <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); var pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); var pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { normal = pointB - pointA; normal.Normalize(); } var cA = pointA + radiusA * normal; var cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); break; } case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); var planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { var clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); var cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; var cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); } break; } case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); var planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { var clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); var cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; var cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. normal = -normal; break; } } }
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) { _step = step; _constraintCount = contactCount; _constraints = new ContactConstraint[_constraintCount]; for (int i = 0; i < _constraintCount; i++) { _constraints[i] = new ContactConstraint(); } //int count = 0; for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); Vec2 vA = bodyA._linearVelocity; Vec2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; Box2DNetDebug.Assert(manifold.PointCount > 0); WorldManifold worldManifold = new WorldManifold(); worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB); ContactConstraint cc = _constraints[i]; cc.BodyA = bodyA; cc.BodyB = bodyB; cc.Manifold = manifold; cc.Normal = worldManifold.Normal; cc.PointCount = manifold.PointCount; cc.Friction = friction; cc.Restitution = restitution; cc.LocalPlaneNormal = manifold.LocalPlaneNormal; cc.LocalPoint = manifold.LocalPoint; cc.Radius = radiusA + radiusB; cc.Type = manifold.Type; unsafe { fixed(ContactConstraintPoint *ccPointsPtr = cc.Points) { for (int j = 0; j < cc.PointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; ContactConstraintPoint *ccp = &ccPointsPtr[j]; ccp->NormalImpulse = cp.NormalImpulse; ccp->TangentImpulse = cp.TangentImpulse; ccp->LocalPoint = cp.LocalPoint; ccp->RA = worldManifold.Points[j] - bodyA._sweep.C; ccp->RB = worldManifold.Points[j] - bodyB._sweep.C; float rnA = Vec2.Cross(ccp->RA, cc.Normal); float rnB = Vec2.Cross(ccp->RB, cc.Normal); rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; Box2DNetDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); ccp->NormalMass = 1.0f / kNormal; float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; Box2DNetDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); ccp->EqualizedMass = 1.0f / kEqualized; Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f); float rtA = Vec2.Cross(ccp->RA, tangent); float rtB = Vec2.Cross(ccp->RB, tangent); rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; Box2DNetDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); ccp->TangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp->VelocityBias = 0.0f; float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA)); if (vRel < -Common.Settings.VelocityThreshold) { ccp->VelocityBias = -cc.Restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc.PointCount == 2) { ContactConstraintPoint *ccp1 = &ccPointsPtr[0]; ContactConstraintPoint *ccp2 = &ccPointsPtr[1]; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; float rn1A = Vec2.Cross(ccp1->RA, cc.Normal); float rn1B = Vec2.Cross(ccp1->RB, cc.Normal); float rn2A = Vec2.Cross(ccp2->RA, cc.Normal); float rn2B = Vec2.Cross(ccp2->RB, cc.Normal); float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 100.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K.Col1.Set(k11, k12); cc.K.Col2.Set(k12, k22); cc.NormalMass = cc.K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } } } } }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (Constraints == null || Constraints.Length < _constraintCount) { Constraints = new ContactConstraint[_constraintCount * 2]; for (int i = 0; i < Constraints.Length; i++) { Constraints[i] = new ContactConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; Debug.Assert(manifold.PointCount > 0); ContactConstraint cc = Constraints[i]; cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); cc.BodyA = bodyA; cc.BodyB = bodyB; cc.Manifold = manifold; cc.Normal = Vector2.Zero; cc.PointCount = manifold.PointCount; cc.LocalNormal = manifold.LocalNormal; cc.LocalPoint = manifold.LocalPoint; cc.RadiusA = radiusA; cc.RadiusB = radiusB; cc.Type = manifold.Type; for (int j = 0; j < cc.PointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; ContactConstraintPoint ccp = cc.Points[j]; if (warmstarting) { ccp.NormalImpulse = impulseRatio * cp.NormalImpulse; ccp.TangentImpulse = impulseRatio * cp.TangentImpulse; } else { ccp.NormalImpulse = 0.0f; ccp.TangentImpulse = 0.0f; } ccp.LocalPoint = cp.LocalPoint; ccp.rA = Vector2.Zero; ccp.rB = Vector2.Zero; ccp.NormalMass = 0.0f; ccp.TangentMass = 0.0f; ccp.VelocityBias = 0.0f; } cc.K.SetZero(); cc.NormalMass.SetZero(); } }
public void InitializeVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint cc = Constraints[i]; float radiusA = cc.RadiusA; float radiusB = cc.RadiusB; Body bodyA = cc.BodyA; Body bodyB = cc.BodyB; Manifold manifold = cc.Manifold; Vector2 vA = bodyA.LinearVelocity; Vector2 vB = bodyB.LinearVelocity; float wA = bodyA.AngularVelocity; float wB = bodyB.AngularVelocity; Debug.Assert(manifold.PointCount > 0); FixedArray2 <Vector2> points; Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB, out cc.Normal, out points); Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X); for (int j = 0; j < cc.PointCount; ++j) { ContactConstraintPoint ccp = cc.Points[j]; ccp.rA = points[j] - bodyA.Sweep.C; ccp.rB = points[j] - bodyB.Sweep.C; float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X; float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X; rnA *= rnA; rnB *= rnB; float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB; Debug.Assert(kNormal > Settings.Epsilon); ccp.NormalMass = 1.0f / kNormal; float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; rtA *= rtA; rtB *= rtB; float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB; Debug.Assert(kTangent > Settings.Epsilon); ccp.TangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.VelocityBias = 0.0f; float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) + cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X); if (vRel < -Settings.VelocityThreshold) { ccp.VelocityBias = -cc.Restitution * vRel; } else { ccp.VelocityBias = 0.0f; } } // If we have two points, then prepare the block solver. if (cc.PointCount == 2) { ContactConstraintPoint ccp1 = cc.Points[0]; ContactConstraintPoint ccp2 = cc.Points[1]; float invMassA = bodyA.InvMass; float invIA = bodyA.InvI; float invMassB = bodyB.InvMass; float invIB = bodyB.InvI; float rn1A = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X; float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X; float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X; float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X; float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 100.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K.Col1.X = k11; cc.K.Col1.Y = k12; cc.K.Col2.X = k12; cc.K.Col2.Y = k22; float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y; float det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; } cc.NormalMass.Col1.X = det * d; cc.NormalMass.Col1.Y = -det * c; cc.NormalMass.Col2.X = -det * b; cc.NormalMass.Col2.Y = det * a; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } } }
/// Evaluate this contact with your own manifold and transforms. internal abstract void Evaluate(ref Manifold manifold, in Transform xfA, Transform xfB);
/// <inheritdoc /> protected override void PostStep() { if (Input.GetKeyDown(KeyCode.A)) { _positionB.X -= 0.1f; } if (Input.GetKeyDown(KeyCode.D)) { _positionB.X += 0.1f; } if (Input.GetKeyDown(KeyCode.S)) { _positionB.Y -= 0.1f; } if (Input.GetKeyDown(KeyCode.W)) { _positionB.Y += 0.1f; } if (Input.GetKeyDown(KeyCode.Q)) { _angleB += 0.1f * Settings.Pi; } if (Input.GetKeyDown(KeyCode.E)) { _angleB -= 0.1f * Settings.Pi; } _transformB.Set(_positionB, _angleB); var manifold = new Manifold(); CollisionUtils.CollidePolygons(ref manifold, _polygonA, _transformA, _polygonB, _transformB); var worldManifold = new WorldManifold(); worldManifold.Initialize(manifold, _transformA, _polygonA.Radius, _transformB, _polygonB.Radius); DrawString($"point count = {manifold.PointCount}"); { var color = Color.FromArgb(230, 230, 230); var v = new Vector2[Settings.MaxPolygonVertices]; for (var i = 0; i < _polygonA.Count; ++i) { v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]); } Drawer.DrawPolygon(v, _polygonA.Count, color); for (var i = 0; i < _polygonB.Count; ++i) { v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]); } Drawer.DrawPolygon(v, _polygonB.Count, color); } for (var i = 0; i < manifold.PointCount; ++i) { Drawer.DrawPoint(worldManifold.Points[i], 4.0f, Color.FromArgb(230, 77, 77)); } }
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB) { Pool.GetCollision().CollideEdgeAndPolygon(manifold, (EdgeShape)FixtureA.Shape, xfA, (PolygonShape)FixtureB.Shape, xfB); }
/// <summary> /// Used to delete a selected coordinate whilst MODETYPE_OPERATION==DeleteCoordinate /// </summary> private void deleteCoordinate(Manifold.Interop.Point p_objPointScreen) { try { if (EditModeOperation == MODETYPE_OPERATION.EditDeleteCoordinate) { Utility.setNearestSelection(this._MapControl, this._DrawingEdit, p_objPointScreen, this._SelectionTolerance); if (this._DrawingEdit.Selection.Count > 0) { switch (this._DrawingOriginalEditObject.Selection[0].Type) { case Manifold.Interop.ObjectType.ObjectArea: deleteAreaCoordinate(); break; case Manifold.Interop.ObjectType.ObjectLine: deleteLineCoordinate(); break; default: break; } } } } catch (Exception objEx) { throw; } }
private void insertEditObjectInflectionPoints(Manifold.Interop.Drawing p_Drawing, int p_ManifoldID, Manifold.Interop.BranchSet p_ManifoldObjectBranchSet) { try { StringBuilder sbSQL = new StringBuilder(); for (int iBranchIndex = 0; iBranchIndex < p_ManifoldObjectBranchSet.Count; iBranchIndex++) { Manifold.Interop.Branch objBranch = p_ManifoldObjectBranchSet.get_Item(iBranchIndex); int iCoorCounts = objBranch.get_PointSet().Count; sbSQL.Length = 0; sbSQL.AppendLine("INSERT INTO [" + this._DrawingEdit.Name + "] "); sbSQL.AppendLine("( "); sbSQL.AppendLine(" PointIndex "); sbSQL.AppendLine(" ,[Geom (I)] "); sbSQL.AppendLine(" ,BranchMaxIndex "); sbSQL.AppendLine(" ,ThemeFeatureType "); sbSQL.AppendLine(" ,[Selection (I)]"); sbSQL.AppendLine(" ,BranchIndex "); sbSQL.AppendLine(") "); sbSQL.AppendLine("SELECT "); sbSQL.AppendLine(" CASE [Point] = StartPoint(ConvertToLine(BRANCH([ID]," + iBranchIndex.ToString() + "))) "); sbSQL.AppendLine(" WHEN TRUE THEN 0 "); sbSQL.AppendLine(" ELSE CoordCount(Branch(IntersectLine(ConvertToLine(BRANCH([ID]," + iBranchIndex.ToString() + ")), [Point]), 0)) - 1 "); sbSQL.AppendLine(" END AS [PointIndex] "); sbSQL.AppendLine(" ,[Point] "); sbSQL.AppendLine(" ,CoordCount(BRANCH([ID]," + iBranchIndex.ToString() + ")) AS [BranchPointMaxIndex] "); sbSQL.AppendLine(" , \"" + this._Edit_ThemePoint + "\""); sbSQL.AppendLine(" , 1"); sbSQL.AppendLine(" , " + iBranchIndex.ToString() + " "); sbSQL.AppendLine("FROM [" + p_Drawing.Name + "] "); sbSQL.AppendLine("WHERE ID = " + p_ManifoldID.ToString() + " "); sbSQL.AppendLine("SPLIT BY Coords(BRANCH([ID]," + iBranchIndex.ToString() + ")) AS [Point]; "); Utility.executeSQL(sbSQL.ToString()); } } catch (Exception objEx) { throw; } }
public ManifoldRecord(Manifold manifold) { this.manifold = manifold; this.isExpanded = false; }
public virtual void preSolve(Contact contact, Manifold oldManifold) { Manifold manifold = contact.getManifold(); if (manifold.pointCount == 0) { return; } Fixture fixtureA = contact.getFixtureA(); Fixture fixtureB = contact.getFixtureB(); Collision.Collision.getPointStates(state1, state2, oldManifold, manifold); contact.getWorldManifold(worldManifold); for (int i = 0; i < manifold.pointCount && pointCount < MAX_CONTACT_POINTS; i++) { ContactPoint cp = points[pointCount]; cp.fixtureA = fixtureA; cp.fixtureB = fixtureB; cp.position.set(worldManifold.points[i]); cp.normal.set(worldManifold.normal); cp.state = state2[i]; cp.normalImpulse = manifold.points[i].normalImpulse; cp.tangentImpulse = manifold.points[i].tangentImpulse; cp.separation = worldManifold.separations[i]; ++pointCount; } }
private static void CollideCircles(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) { Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); }
public void OnCollide(Manifold m) { Node otherNode = m.A != Collision ? m.A : m.B; if (otherNode.Parent is Asteroid) { Destroy(); } }
/// <summary> /// Collides the polygons using the specified manifold /// </summary> /// <param name="manifold">The manifold</param> /// <param name="shape1">The shape</param> /// <param name="xf1">The xf</param> /// <param name="shape2">The shape</param> /// <param name="xf2">The xf</param> private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2); }
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB) { pool.getCollision().collideEdgeAndCircle(manifold, (EdgeShape) m_fixtureA.getShape(), xfA, (CircleShape) m_fixtureB.getShape(), xfB); }
/// <summary> /// Initialize position dependent portions of the velocity constraints. /// </summary> public void InitializeVelocityConstraints() { for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; ContactPositionConstraint pc = _positionConstraints[i]; float radiusA = pc.RadiusA; float radiusB = pc.RadiusB; Manifold manifold = _contacts[vc.ContactIndex].Manifold; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; Vector2 localCenterA = pc.LocalCenterA; Vector2 localCenterB = pc.LocalCenterB; Vector2 cA = _positions[indexA].C; float aA = _positions[indexA].A; Vector2 vA = _velocities[indexA].V; float wA = _velocities[indexA].W; Vector2 cB = _positions[indexB].C; float aB = _positions[indexB].A; Vector2 vB = _velocities[indexB].V; float wB = _velocities[indexB].W; Debug.Assert(manifold.PointCount > 0); Transform xfA = new Transform(); Transform xfB = new Transform(); xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); Vector2 normal; FixedArray2 <Vector2> points; FixedArray2 <float> _; WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points, out _); vc.Normal = normal; int pointCount = vc.PointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.Points[j]; vcp.rA = points[j] - cA; vcp.rB = points[j] - cB; float rnA = MathUtils.Cross(vcp.rA, vc.Normal); float rnB = MathUtils.Cross(vcp.rB, vc.Normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vector2 tangent = MathUtils.Cross(vc.Normal, 1.0f); float rtA = MathUtils.Cross(vcp.rA, tangent); float rtB = MathUtils.Cross(vcp.rB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.VelocityBias = 0.0f; float vRel = Vector2.Dot(vc.Normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA)); if (vRel < -Settings.VelocityThreshold) { vcp.VelocityBias = -vc.Restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.PointCount == 2 && Settings.BlockSolve) { VelocityConstraintPoint vcp1 = vc.Points[0]; VelocityConstraintPoint vcp2 = vc.Points[1]; float rn1A = MathUtils.Cross(vcp1.rA, vc.Normal); float rn1B = MathUtils.Cross(vcp1.rB, vc.Normal); float rn2A = MathUtils.Cross(vcp2.rA, vc.Normal); float rn2B = MathUtils.Cross(vcp2.rB, vc.Normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.ex = new Vector2(k11, k12); vc.K.ey = new Vector2(k12, k22); vc.NormalMass = vc.K.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.PointCount = 1; } } } }
public override void preSolve(Contact contact, Manifold oldManifold) { base.preSolve(contact, oldManifold); Fixture fixtureA = contact.getFixtureA(); Fixture fixtureB = contact.getFixtureB(); if (fixtureA == m_platform || fixtureB == m_platform) { contact.setTangentSpeed(5.0f); } }
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities) { _step = step; _count = count; _positions = positions; _velocities = velocities; _contacts = contacts; // grow the array if (VelocityConstraints == null || VelocityConstraints.Length < count) { VelocityConstraints = new ContactVelocityConstraint[count * 2]; _positionConstraints = new ContactPositionConstraint[count * 2]; for (int i = 0; i < VelocityConstraints.Length; i++) { VelocityConstraints[i] = new ContactVelocityConstraint(); } for (int i = 0; i < _positionConstraints.Length; i++) { _positionConstraints[i] = new ContactPositionConstraint(); } } // Initialize position independent portions of the constraints. for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = VelocityConstraints[i]; vc.Friction = contact.Friction; vc.Restitution = contact.Restitution; vc.TangentSpeed = contact.TangentSpeed; vc.IndexA = bodyA.IslandIndex; vc.IndexB = bodyB.IslandIndex; vc.InvMassA = bodyA._invMass; vc.InvMassB = bodyB._invMass; vc.InvIA = bodyA._invI; vc.InvIB = bodyB._invI; vc.ContactIndex = i; vc.PointCount = pointCount; vc.K.SetZero(); vc.NormalMass.SetZero(); ContactPositionConstraint pc = _positionConstraints[i]; pc.IndexA = bodyA.IslandIndex; pc.IndexB = bodyB.IslandIndex; pc.InvMassA = bodyA._invMass; pc.InvMassB = bodyB._invMass; pc.LocalCenterA = bodyA._sweep.LocalCenter; pc.LocalCenterB = bodyB._sweep.LocalCenter; pc.InvIA = bodyA._invI; pc.InvIB = bodyB._invI; pc.LocalNormal = manifold.LocalNormal; pc.LocalPoint = manifold.LocalPoint; pc.PointCount = pointCount; pc.RadiusA = radiusA; pc.RadiusB = radiusB; pc.Type = manifold.Type; for (int j = 0; j < pointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; VelocityConstraintPoint vcp = vc.Points[j]; if (Settings.EnableWarmstarting) { vcp.NormalImpulse = _step.dtRatio * cp.NormalImpulse; vcp.TangentImpulse = _step.dtRatio * cp.TangentImpulse; } else { vcp.NormalImpulse = 0.0f; vcp.TangentImpulse = 0.0f; } vcp.rA = Vector2.Zero; vcp.rB = Vector2.Zero; vcp.NormalMass = 0.0f; vcp.TangentMass = 0.0f; vcp.VelocityBias = 0.0f; pc.LocalPoints[j] = cp.LocalPoint; } } }
private bool GetNextCollision(IReadOnlyList <Manifold> collisions, int counter, out Manifold collision) { // The *4 is completely arbitrary if (counter > collisions.Count * 4) { collision = default; return(false); } var indexes = new List <int>(); for (int i = 0; i < collisions.Count; i++) { indexes.Add(i); } _random.Shuffle(indexes); foreach (var index in indexes) { if (collisions[index].Unresolved) { collision = collisions[index]; return(true); } } collision = default; return(false); }
public override void HandleCollision(Manifold m, Body a, Body b) { Circle A = (Circle)a.shape; Polygon B = (Polygon)b.shape; m.contactCount = 0; // Transform circle center to Polygon model space // Vec2 center = a->position; // center = B->u.Transpose( ) * (center - b->position); Vec2 center = B.u.Transpose().Muli(a.position.Sub(b.position)); // Find edge with minimum penetration // Exact concept as using support points in Polygon vs Polygon float separation = -float.MaxValue; int faceNormal = 0; for (int i = 0; i < B.vertexCount; ++i) { // real s = Dot( B->m_normals[i], center - B->m_vertices[i] ); float s = Vec2.Dot(B.normals[i], center.Sub(B.vertices[i])); if (s > A.radius) { return; } if (s > separation) { separation = s; faceNormal = i; } } // Grab face's vertices Vec2 v1 = B.vertices[faceNormal]; int i2 = faceNormal + 1 < B.vertexCount ? faceNormal + 1 : 0; Vec2 v2 = B.vertices[i2]; // Check to see if center is within polygon if (separation < ImpulseMath.EPSILON) { // m->contact_count = 1; // m->normal = -(B->u * B->m_normals[faceNormal]); // m->contacts[0] = m->normal * A->radius + a->position; // m->penetration = A->radius; m.contactCount = 1; B.u.Mul(B.normals[faceNormal], m.normal).Negi(); m.contacts[0].Set(m.normal).Muli(A.radius).Addi(a.position); m.penetration = A.radius; return; } // Determine which voronoi region of the edge center of circle lies within // real dot1 = Dot( center - v1, v2 - v1 ); // real dot2 = Dot( center - v2, v1 - v2 ); // m->penetration = A->radius - separation; float dot1 = Vec2.Dot(center.Sub(v1), v2.Sub(v1)); float dot2 = Vec2.Dot(center.Sub(v2), v1.Sub(v2)); m.penetration = A.radius - separation; // Closest to v1 if (dot1 <= 0.0f) { if (Vec2.DistanceSq(center, v1) > A.radius * A.radius) { return; } // m->contact_count = 1; // Vec2 n = v1 - center; // n = B->u * n; // n.Normalize( ); // m->normal = n; // v1 = B->u * v1 + b->position; // m->contacts[0] = v1; m.contactCount = 1; B.u.Muli(m.normal.Set(v1).Subi(center)).Normalize(); B.u.Mul(v1, m.contacts[0]).Addi(b.position); } // Closest to v2 else if (dot2 <= 0.0f) { if (Vec2.DistanceSq(center, v2) > A.radius * A.radius) { return; } // m->contact_count = 1; // Vec2 n = v2 - center; // v2 = B->u * v2 + b->position; // m->contacts[0] = v2; // n = B->u * n; // n.Normalize( ); // m->normal = n; m.contactCount = 1; B.u.Muli(m.normal.Set(v2).Subi(center)).Normalize(); B.u.Mul(v2, m.contacts[0]).Addi(b.position); } // Closest to face else { Vec2 n = B.normals[faceNormal]; if (Vec2.Dot(center.Sub(v1), n) > A.radius) { return; } // n = B->u * n; // m->normal = -n; // m->contacts[0] = m->normal * A->radius + a->position; // m->contact_count = 1; m.contactCount = 1; B.u.Mul(n, m.normal).Negi(); m.contacts[0].Set(a.position).Addsi(m.normal, A.radius); } }
/// <summary> /// /// </summary> /// <param name="p_MapControl">Reference to the Manifold ActiveX control.</param> /// <param name="p_DrawingOriginalEditObject">The Drawing layer which contains the original Line\Area to edit.</param> /// <param name="p_DrawingEdit">A temporary Drawing layer - used only to display inflection Points of the Line\area to edit.</param> /// <param name="p_Draw">Internal class used to draw ojects on Drawings.</param> /// <param name="p_MODETYPE_OPERATION"></param> public EditDrawingFactorySQL(AxManifold.Interop.AxComponentControl p_MapControl, Manifold.Interop.Drawing p_DrawingOriginalEditObject, Manifold.Interop.Drawing p_DrawingEdit, MODETYPE_OPERATION p_MODETYPE_OPERATION) { try { //Map events this._MapControl = p_MapControl; this._MapControl.MouseDownEvent += new AxManifold.Interop.IComponentControlEvents_MouseDownEventHandler(this.MapControl_MouseDownEvent); this._MapControl.MouseUpEvent += new AxManifold.Interop.IComponentControlEvents_MouseUpEventHandler(this.MapControl_MouseUpEvent); this._MapControl.MouseMoveEvent += new AxManifold.Interop.IComponentControlEvents_MouseMoveEventHandler(this.MapControl_MouseMoveEvent); if (p_DrawingOriginalEditObject == null ) { throw new Exception("The Original Edit objects Drawing was null in the EditDrawingFactory constructor. Assign an initialised Manifold.Interop.Drawing to it before calling this class."); } else { this._DrawingOriginalEditObject = p_DrawingOriginalEditObject; } if (p_DrawingEdit == null) { throw new Exception("The Edit Drawing layer was null in the EditDrawingFactory constructor. Assign an initialised Manifold.Interop.Drawing to it before calling this class."); } else { this._DrawingEdit = p_DrawingEdit; } this._EditModeOperation = p_MODETYPE_OPERATION; clearEditLayer(); } catch (Exception objEx) { throw; } }
protected virtual void PreSolve(Contact contact, ref Manifold oldManifold) { }
/// <summary> /// Adds a coordinate to the selected Edit object /// </summary> private void addCoordinate1(Manifold.Interop.Point p_objPointScreen) { Manifold.Interop.Query query; bool bAddPointAtEnd = false; this._AddingCoordinate = true; try { bool bAdditionOccured = false; if (this._DrawingOriginalEditObject.Selection.Count > 0) { StringBuilder sbSQL = new StringBuilder(); double SearchRange = this._SelectionTolerance; ; double AddCoordinateSelectionTolerance = this._SelectionToleranceAddCoordinate; query = this._MapControl.get_Document().NewQuery("TempQuery", false); //Compose a rectangular area around the clicked point - then gets its Geom WKT Manifold.Interop.PointSet points = this._MapControl.Application.NewPointSet(); points.Add(this._MapControl.Application.NewPoint(p_objPointScreen.X - SearchRange, p_objPointScreen.Y - SearchRange)); points.Add(this._MapControl.Application.NewPoint(p_objPointScreen.X + SearchRange, p_objPointScreen.Y - SearchRange)); points.Add(this._MapControl.Application.NewPoint(p_objPointScreen.X + SearchRange, p_objPointScreen.Y + SearchRange)); points.Add(this._MapControl.Application.NewPoint(p_objPointScreen.X - SearchRange, p_objPointScreen.Y + SearchRange)); points.Add(VBManifoldWrapper.ManifoldObjectWrapper.getPointFromPointSet(points, 0)); Manifold.Interop.Geom geom = this._MapControl.Application.NewGeom(Manifold.Interop.GeomType.GeomArea, null); Manifold.Interop.BranchSet geomBranchSet = geom.get_BranchSet(); VBManifoldWrapper.ManifoldObjectWrapper.setPointSetInBranchSet(points, geomBranchSet); string wkt = geom.ToTextWKT(); //Create a temp Drawing to hold a copy of the object which will be Exploded into its separate Line segments Manifold.Interop.Drawing tempDrawing = this._MapControl.get_Document().NewDrawing("tempDrawing", this._MapControl.Application.NewCoordinateSystem("Irish Grid"), false); this._DrawingOriginalEditObject.Copy(true); tempDrawing.Paste(true); //NOTE: When the object is exploded the Points at the end of the lines dont match exactely (percision // is slightly off) to those that were added from the objects PointSet to the Edit Drawing layer thus // query below adds small BUFFER and TOUCHES logic to get around this. Manifold.Interop.Analyzer myAnalyzer = this._MapControl.get_Document().NewAnalyzer(); myAnalyzer.Boundaries((Manifold.Interop.Component)tempDrawing, tempDrawing, tempDrawing.ObjectSet); tempDrawing.SelectInverse(); tempDrawing.Clear(true); myAnalyzer.Explode((Manifold.Interop.Component)tempDrawing, tempDrawing.ObjectSet); sbSQL.Length = 0; sbSQL.AppendLine("SELECT [BranchIndex], [PointIndex], [BranchMaxIndex] FROM [" + this._DrawingEdit.Name + "] "); sbSQL.AppendLine("Where TOUCHES([ID],BUFFER( "); sbSQL.AppendLine("Select StartPoint(Geom(ID)) as s FROM [" + tempDrawing.Name + "] "); sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); sbSQL.AppendLine("UNION "); sbSQL.AppendLine("SELECT [BranchIndex], [PointIndex], [BranchMaxIndex] FROM [" + this._DrawingEdit.Name + "] "); sbSQL.AppendLine("Where TOUCHES([ID],BUFFER( "); sbSQL.AppendLine("Select EndPoint(Geom(ID)) as s FROM [" + tempDrawing.Name + "] "); sbSQL.AppendLine("WHERE Touches(AssignCoordSys(CGeom(CGeomWKB(\"" + wkt + "\")), CoordSys(\"" + this._DrawingEdit.Name + "\" AS COMPONENT)), [ID])," + AddCoordinateSelectionTolerance.ToString() + ")) "); sbSQL.AppendLine("ORDER BY [PointIndex] ASC "); query.Text = sbSQL.ToString(); query.Run(); Manifold.Interop.Table table = query.Table; Int32 iBranchIndex = 0; Int32 iFirstPointIndex = 0; Int32 iLastPointIndex; Int32 iBranchMaxIndex; if (table.RecordSet.Count > 0) { iBranchIndex = Convert.ToInt32(table.RecordSet[0].get_Data("BranchIndex")); iBranchMaxIndex = Convert.ToInt32(table.RecordSet[0].get_Data("BranchMaxIndex")); iFirstPointIndex = Convert.ToInt32(table.RecordSet[0].get_Data("PointIndex")); iLastPointIndex = Convert.ToInt32(table.RecordSet[1].get_Data("PointIndex")); if (iFirstPointIndex != iLastPointIndex) { Manifold.Interop.Branch objBranch = VBManifoldWrapper.ManifoldObjectWrapper.getBranch(this._OriginalObjectBranchSet, iBranchIndex); Manifold.Interop.PointSet objPointSetBefore = objBranch.get_PointSet(); Manifold.Interop.PointSet objPointSetAfter = this._MapControl.get_Document().Application.NewPointSet(); //Remove Query and tempDrawing tempDrawing.Document.ComponentSet.Remove("tempDrawing"); this._DrawingOriginalEditObject.Document.ComponentSet.Remove(query); //When adding point to end of GEOM i.e.- FirstIndex=0 and LastIndex=PointSet.Count // this dosnt work so dont add any points - this will bve done later. if (iFirstPointIndex == 0 && iLastPointIndex == objPointSetBefore.Count - 1) { bAddPointAtEnd = true; } else { //Determine where to insert the point for (int iIndex = 0; iIndex < objPointSetBefore.Count; iIndex++) { if (bAdditionOccured == false) { if (iIndex > iFirstPointIndex) { objPointSetAfter.Add(p_objPointScreen); bAdditionOccured = true; } } objPointSetAfter.Add(VBManifoldWrapper.ManifoldObjectWrapper.getPointFromPointSet(objPointSetBefore, iIndex)); } } //Redraw the new Object with added Point if required if (bAdditionOccured == true) { objBranch.set_PointSet(objPointSetAfter); prepareEditObject(); } else { if (bAddPointAtEnd == true) { objPointSetBefore.Add(p_objPointScreen); objBranch.set_PointSet(objPointSetBefore); prepareEditObject(); } } } } //Unset any Edit Drawing selection this._DrawingEdit.SelectNone(); this._UserAddedCoordinate = true; } } catch (Exception objEx) { throw; } finally { this._AddingCoordinate = false; } }
/// <summary> /// Updates the listener /// </summary> /// <param name="listener">The listener</param> public void Update(IContactListener listener) { Manifold oldManifold = Manifold.Clone(); Evaluate(); Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; int oldCount = oldManifold.PointCount; int newCount = Manifold.PointCount; if (newCount == 0 && oldCount > 0) { bodyA.WakeUp(); bodyB.WakeUp(); } // Slow contacts don't generate TOI events. if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet()) { Flags &= ~CollisionFlags.Slow; } else { Flags |= CollisionFlags.Slow; } // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < Manifold.PointCount; ++i) { ManifoldPoint mp2 = Manifold.Points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; ContactId id2 = mp2.Id; for (int j = 0; j < oldManifold.PointCount; ++j) { ManifoldPoint mp1 = oldManifold.Points[j]; if (mp1.Id.Key == id2.Key) { mp2.NormalImpulse = mp1.NormalImpulse; mp2.TangentImpulse = mp1.TangentImpulse; break; } } } if (oldCount == 0 && newCount > 0) { Flags |= CollisionFlags.Touch; if (listener != null) { listener.BeginContact(this); } } if (oldCount > 0 && newCount == 0) { Flags &= ~CollisionFlags.Touch; if (listener != null) { listener.EndContact(this); } } if ((Flags & CollisionFlags.NonSolid) == 0) { if (listener != null) { listener.PreSolve(this, oldManifold); } // The user may have disabled contact. if (Manifold.PointCount == 0) { Flags &= ~CollisionFlags.Touch; } } }
private void insertEditObjectInflectionPoints(Manifold.Interop.Drawing p_Drawing, int p_ManifoldID) { try { Manifold.Interop.Table objTable = (Manifold.Interop.Table)p_Drawing.OwnedTable; StringBuilder sbSQL = new StringBuilder(); sbSQL.AppendLine("INSERT INTO [" + this._DrawingEdit.Name + "] "); sbSQL.AppendLine("( "); sbSQL.AppendLine(" PointIndex "); sbSQL.AppendLine(" ,[Geom (I)] "); sbSQL.AppendLine(" ,BranchMaxIndex "); sbSQL.AppendLine(" ,ThemeFeatureType "); sbSQL.AppendLine(" ,[Selection (I)]"); sbSQL.AppendLine(") "); sbSQL.AppendLine("SELECT "); sbSQL.AppendLine(" CASE [Point] = StartPoint(ConvertToLine([ID])) "); sbSQL.AppendLine(" WHEN TRUE THEN 0 "); sbSQL.AppendLine(" ELSE CoordCount(Branch(IntersectLine(ConvertToLine([ID]), [Point]), 0)) - 1 "); sbSQL.AppendLine(" END AS [PointIndex] "); sbSQL.AppendLine(" ,[Point] "); sbSQL.AppendLine(" ,CoordCount(ID) AS [BranchPointMaxIndex] "); sbSQL.AppendLine(" , \"" + this._Edit_ThemePoint + "\""); sbSQL.AppendLine(" , 1"); sbSQL.AppendLine("FROM [" + p_Drawing.Name + "] "); sbSQL.AppendLine("WHERE ID = " + p_ManifoldID.ToString() + " "); sbSQL.AppendLine("SPLIT BY Coords([ID]) AS [Point]; "); Utility.executeSQL(sbSQL.ToString()); } catch (Exception objEx) { throw; } }
private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) { Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2); }
public abstract void evaluate(Manifold manifold, Transform xfA, Transform xfB);
public void PreSolve(Contact contact, Manifold manifold) { }
internal void PreSolve(Contact contact, ref Manifold oldManifold) { //CreateContactInfo(contact, ContactType.PreSolve, ContactType.PreSolve); }
/// <summary> /// Update the contact manifold and touching status. /// Note: do not assume the fixture AABBs are overlapping or are valid. /// </summary> /// <param name="contactManager">The contact manager.</param> internal void Update(ContactManager contactManager) { Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; if (FixtureA == null || FixtureB == null) { return; } Manifold oldManifold = Manifold; // Re-enable this contact. Enabled = true; bool touching; bool wasTouching = IsTouching; bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; // Is this contact a sensor? if (sensor) { Shape shapeA = FixtureA.Shape; Shape shapeB = FixtureB.Shape; touching = Collision.Collision.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA._xf, ref bodyB._xf); // Sensors don't generate manifolds. Manifold.PointCount = 0; } else { Evaluate(ref Manifold, ref bodyA._xf, ref bodyB._xf); touching = Manifold.PointCount > 0; // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < Manifold.PointCount; ++i) { ManifoldPoint mp2 = Manifold.Points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; ContactID id2 = mp2.Id; for (int j = 0; j < oldManifold.PointCount; ++j) { ManifoldPoint mp1 = oldManifold.Points[j]; if (mp1.Id.Key == id2.Key) { mp2.NormalImpulse = mp1.NormalImpulse; mp2.TangentImpulse = mp1.TangentImpulse; break; } } Manifold.Points[i] = mp2; } if (touching != wasTouching) { bodyA.Awake = true; bodyB.Awake = true; } } IsTouching = touching; if (wasTouching == false) { if (touching) { #if (AllCollisionCallbacksAgree) { bool enabledA = true, enabledB = true; // Report the collision to both participants. Track which ones returned true so we can // later call OnSeparation if the contact is disabled for a different reason. enabledA = FixtureA.RaiseOnCollision(FixtureA, FixtureB, this) && enabledA; // Reverse the order of the reported fixtures. The first fixture is always the one that the // user subscribed to. enabledB = FixtureB.RaiseOnCollision(FixtureB, FixtureA, this) && enabledB; Enabled = enabledA && enabledB; // BeginContact can also return false and disable the contact if (enabledA && enabledB && contactManager.BeginContact != null) { Enabled = contactManager.BeginContact(this); } } #else { //Report the collision to both participants: Enabled = FixtureA.RaiseOnCollision(FixtureA, FixtureB, this); //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. Enabled = FixtureB.RaiseOnCollision(FixtureB, FixtureA, this); //BeginContact can also return false and disable the contact if (contactManager.BeginContact != null) { Enabled = contactManager.BeginContact(this); } } #endif // If the user disabled the contact (needed to exclude it in TOI solver) at any point by // any of the callbacks, we need to mark it as not touching and call any separation // callbacks for fixtures that didn't explicitly disable the collision. if (!Enabled) { IsTouching = false; } } } else { if (touching == false) { //Report the separation to both participants: FixtureA?.RaiseOnSeparation(FixtureA, FixtureB); //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. FixtureB?.RaiseOnSeparation(FixtureB, FixtureA); contactManager.EndContact?.Invoke(this); } } if (sensor) { return; } if (contactManager.PreSolve != null) { contactManager.PreSolve(this, ref oldManifold); } }
internal override void Evaluate(out Manifold manifold, ref Transform xfA, ref Transform xfB) { Collision.CollideCircles(out manifold, (CircleShape)FixtureA.Shape, ref xfA, (CircleShape)FixtureB.Shape, ref xfB); }
/// An axis aligned bounding box. /// Compute the collision manifold between two circles. public static void CollideCircles( ref Manifold manifold, CircleShape circleA, in Transform xfA,
public override void preSolve(Contact contact, Manifold oldManifold) { base.preSolve(contact, oldManifold); Fixture fixtureA = contact.getFixtureA(); Fixture fixtureB = contact.getFixtureB(); if (fixtureA != m_platform && fixtureA != m_character) { return; } if (fixtureB != m_character && fixtureB != m_character) { return; } Vec2 position = m_character.getBody().getPosition(); if (position.y < m_top + m_radius - 3.0f*Settings.linearSlop) { contact.setEnabled(false); } }
public void PhysicsUpdate() { for (int i = 0; i < physEnts.Length; i++) { if (physEnts[i] == null) { continue; } for (int j = i + 1; j < physEnts.Length; j++) { if (physEnts[j] == null) { continue; } PhysicsData a = physEnts[i]; PhysicsData b = physEnts[j]; Manifold m = new Manifold { a = a, b = b }; if (CircleVsCircle(m, a.circ, b.circ)) { Calc.ResolveCollision(a, b, m.normal, m.penetration); } // do collision stuff for a and b? } } // gravity for (int i = 0; i < physEnts.Length; i++) { if (physEnts[i] == null) { continue; } for (int j = i + 1; j < physEnts.Length; j++) { if (physEnts[j] == null) { continue; } Vector2 forceA = Calc.GravityForce(physEnts[i], physEnts[j]); physEnts[i].velocity += forceA / physEnts[i].mass * (1f / 60f); Vector2 forceB = Calc.GravityForce(physEnts[j], physEnts[i]); physEnts[j].velocity += forceB / physEnts[j].mass * (1f / 60f); } } for (int i = 0; i < physEnts.Length; i++) { if (physEnts[i] == null || physEnts[i].mass == 0) { continue; } physEnts[i].position += (Vector3)(physEnts[i].velocity) * (1.0f / 60.0f); physEnts[i].owner.transform.position = physEnts[i].position; } }
public override void PreSolve(Contact contact, ref Manifold oldManifold) { base.PreSolve(contact, ref oldManifold); Fixture fixtureA = contact.GetFixtureA(); Fixture fixtureB = contact.GetFixtureB(); if (fixtureA != _platform && fixtureA != _character) { return; } if (fixtureB != _character && fixtureB != _character) { return; } Vector2 position = _character.GetBody().GetPosition(); if (position.Y < _top + _radius - 3.0f * Settings.b2_linearSlop) { contact.SetEnabled(false); } }
internal override void Evaluate(out Manifold manifold, in Transform xfA, in Transform xfB)
/// <summary> /// A fallback for handling degenerate "Star of David" cases. /// </summary> private static void FindVertsFallback( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPointPartial(vertex, normal) == true) if (manifold.AddContact(vertex, normal, penetration) == false) return; } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPointPartial(vertex, -normal) == true) if (manifold.AddContact(vertex, normal, penetration) == false) return; } }
/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// 计算边缘和圆的碰撞点 /// </summary> /// <param name="manifold"></param> /// <param name="edgeA"></param> /// <param name="xfA"></param> /// <param name="circleB"></param> /// <param name="xfB"></param> public static void CollideEdgeAndCircle( ref Manifold manifold, EdgeShape edgeA, in Transform xfA,
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB) { pool.getCollision().collidePolygons(manifold, (PolygonShape) m_fixtureA.getShape(), xfA, (PolygonShape) m_fixtureB.getShape(), xfB); }
/// <summary> /// Checks whether the given colliders intersect, generating an Intersection instance /// with the collision data in that case. /// Since this function represents a "box vs circle" collision, the symmetric function is called /// instead. See DCircleCollider for more info. /// </summary> /// <param name="other">the second collider</param> /// <param name="intersection">the collision data, <code>null</code> if no collision has been detected.</param> /// <returns></returns> public override bool Intersects(DCircleCollider other, out Manifold intersection) { return(other.Intersects(this, out intersection)); }
public static bool AABBvsAABB(Manifold M) { // Setup manifold object Brick A = (Brick)M.m_A; Brick B = (Brick)M.m_B; // Vector from A to B //Vector2 n = B.m_position - A.m_position; Vector2 posA = new Vector2((A.m_position.X + (A.m_texture.Width / 2)), (A.m_position.Y + (A.m_texture.Height / 2))); Vector2 posB = new Vector2((B.m_position.X + (B.m_texture.Width / 2)), (B.m_position.Y + (B.m_texture.Height / 2))); Vector2 n = posB - posA; AABB abox = (AABB)A.m_hitbox; AABB bbox = (AABB)B.m_hitbox; // Calculate half extents along x axis for each object float a_extent = (abox.m_max.X - abox.m_min.X) / 2; float b_extent = (bbox.m_max.X - bbox.m_min.X) / 2; // Calculate overlap on x axis float x_overlap = a_extent + b_extent - Math.Abs(n.X); // SAT test on x axis if (x_overlap > 0) { // Calculate half extents along x axis for each object a_extent = (abox.m_max.Y - abox.m_min.Y) / 2; b_extent = (bbox.m_max.Y - bbox.m_min.Y) / 2; // Calculate overlap on y axis float y_overlap = a_extent + b_extent - Math.Abs(n.Y); // SAT test on y axis if (y_overlap > 0) { // Find out which axis is axis of least penetration if (x_overlap < y_overlap) { // Point towards B knowing that n points from A to B if (n.X < 0) M.m_normal = new Vector2(-1, 0); else M.m_normal = new Vector2(1, 0); M.m_penetration = x_overlap; return true; } else { // Point toward B knowing that n points from A to B if (n.Y < 0) M.m_normal = new Vector2(0, -1); else M.m_normal = new Vector2(0, 1); M.m_penetration = y_overlap; return true; } } } return false; }
/// <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; }
private void DrawLine(Manifold.Interop.Geom p_objGeomLine) { try { this.Cursor = Cursors.WaitCursor; Utility.DrawLine(this.DrawingOriginal, p_objGeomLine); this.DrawingOriginal.SelectNone(); axComponentControl1.Refresh(); } catch (Exception err) { MessageBox.Show("ERROR:" + err.Message); } finally { this.Cursor = Cursors.Default; } }
/// <summary> /// Update the contact manifold and touching status. /// Note: do not assume the fixture AABBs are overlapping or are valid. /// </summary> /// <param name="contactManager">The contact manager.</param> internal void Update(ContactManager contactManager) { Manifold oldManifold = Manifold; // Re-enable this contact. Flags |= ContactFlags.Enabled; bool touching; bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching; bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; // Is this contact a sensor? if (sensor) { Shape shapeA = FixtureA.Shape; Shape shapeB = FixtureB.Shape; touching = AABB.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA.Xf, ref bodyB.Xf); // Sensors don't generate manifolds. Manifold.PointCount = 0; } else { Evaluate(ref Manifold, ref bodyA.Xf, ref bodyB.Xf); touching = Manifold.PointCount > 0; // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < Manifold.PointCount; ++i) { ManifoldPoint mp2 = Manifold.Points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; ContactID id2 = mp2.Id; bool found = false; for (int j = 0; j < oldManifold.PointCount; ++j) { ManifoldPoint mp1 = oldManifold.Points[j]; if (mp1.Id.Key == id2.Key) { mp2.NormalImpulse = mp1.NormalImpulse; mp2.TangentImpulse = mp1.TangentImpulse; found = true; break; } } if (found == false) { mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; } Manifold.Points[i] = mp2; } if (touching != wasTouching) { bodyA.Awake = true; bodyB.Awake = true; } } if (touching) { Flags |= ContactFlags.Touching; } else { Flags &= ~ContactFlags.Touching; } if (wasTouching == false && touching) { //Report the collision to both participants: if (FixtureA != null) { if (FixtureA.OnCollision != null) { Enabled = FixtureA.OnCollision(FixtureA, FixtureB, this); } } //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. if (FixtureB != null) { if (FixtureB.OnCollision != null) { Enabled = FixtureB.OnCollision(FixtureB, FixtureA, this); } } //BeginContact can also return false and disable the contact if (contactManager.BeginContact != null) { Enabled = contactManager.BeginContact(this); } //if the user disabled the contact (needed to exclude it in TOI solver), we also need to mark //it as not touching. if (Enabled == false) { Flags &= ~ContactFlags.Touching; } } if (wasTouching && touching == false) { //Report the separation to both participants: if (FixtureA != null && FixtureA.OnSeparation != null) { FixtureA.OnSeparation(FixtureA, FixtureB); } //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. if (FixtureB != null && FixtureB.OnSeparation != null) { FixtureB.OnSeparation(FixtureB, FixtureA); } if (contactManager.EndContact != null) { contactManager.EndContact(this); } } if (sensor) { return; } if (contactManager.PreSolve != null) { contactManager.PreSolve(this, ref oldManifold); } }
public virtual void PreSolve(Contact contact, ref Manifold oldManifold) { Manifold manifold; contact.GetManifold(out manifold); if (manifold._pointCount == 0) { return; } Fixture fixtureA = contact.GetFixtureA(); Fixture fixtureB = contact.GetFixtureB(); FixedArray2<PointState> state1, state2; Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); WorldManifold worldManifold; contact.GetWorldManifold(out worldManifold); for (int i = 0; i < manifold._pointCount && _pointCount < k_maxContactPoints; ++i) { if (fixtureA == null) { _points[i] = new ContactPoint(); } ContactPoint cp = _points[_pointCount]; cp.fixtureA = fixtureA; cp.fixtureB = fixtureB; cp.position = worldManifold._points[i]; cp.normal = worldManifold._normal; cp.state = state2[i]; _points[_pointCount] = cp; ++_pointCount; } }
/// <summary> /// Checks whether the two boxes collide, generating an intersection containing the collision data. /// This function can only compare bounding boxes againsts other bounding boxes. /// </summary> /// <param name="other">bounding box to check</param> /// <param name="intersection">the collision data, <code>null</code> if no collision has been detected.</param> /// <returns></returns> public override bool Intersects(DBoxCollider other, out Manifold intersection) { intersection = null; //check if one of them is a trigger if (this.IsTrigger || other.IsTrigger) { intersection = new Manifold(this.Body, other.Body); return(true); } // Vector from A to B Vector2F distance = other.Body.Position - Body.Position; // Calculate half extents along x axis for each object Fix32 xEntentA = (max.x - min.x) / (Fix32)2; Fix32 xExtentB = (other.max.x - other.min.x) / (Fix32)2; // Calculate overlap on x axis Fix32 offsetX = xEntentA + xExtentB - Fix32.Abs(distance.x); // SAT test on x axis if (offsetX > Fix32.Zero) { // Calculate half extents along x axis for each object Fix32 yExtentA = (max.y - min.y) / (Fix32)2; Fix32 yExtentB = (other.max.y - other.min.y) / (Fix32)2; // Calculate overlap on y axis Fix32 offsetY = yExtentA + yExtentB - Fix32.Abs(distance.y); // SAT test on y axis if (offsetY > Fix32.Zero) { Vector2F n; // Find out which axis is axis of least penetration if (offsetX < offsetY) { // Point towards B knowing that n points from A to B if (distance.x < Fix32.Zero) { n = new Vector2F(-1, 0); } else { n = new Vector2F(1, 0); } intersection = new Manifold(Body, other.Body, n, offsetX); return(true); } else { // Point toward B knowing that n points from A to B if (distance.y < Fix32.Zero) { n = new Vector2F(0, -1); } else { n = new Vector2F(0, 1); } intersection = new Manifold(Body, other.Body, n, offsetY); return(true); } } } return(false); }