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);
}
