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