/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
/// <param name="manifold"></param>
/// <param name="circleA"></param>
/// <param name="xfA"></param>
/// <param name="circleB"></param>
/// <param name="xfB"></param>
public static void CollideCircles(ref Manifold manifold,
CircleShape circleA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold._pointCount = 0;
Vector2 pA = MathUtils.Multiply(ref xfA, circleA._p);
Vector2 pB = MathUtils.Multiply(ref xfB, circleB._p);
Vector2 d = pB - pA;
float distSqr = Vector2.Dot(d, d);
float rA = circleA._radius;
float rB = circleB._radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold._type = ManifoldType.Circles;
manifold._localPoint = circleA._p;
manifold._localNormal = Vector2.Zero;
manifold._pointCount = 1;
var p0 = manifold._points[0];
p0.LocalPoint = circleB._p;
p0.Id.Key = 0;
manifold._points[0] = p0;
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygonA"></param>
/// <param name="xfA"></param>
/// <param name="circleB"></param>
/// <param name="xfB"></param>
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygonA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold._pointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Multiply(ref xfB, circleB._p);
Vector2 cLocal = MathUtils.MultiplyT(ref xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.b2_maxFloat;
float radius = polygonA._radius + circleB._radius;
int vertexCount = polygonA._vertexCount;
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.b2_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._p;
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._p;
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._p;
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._p;
p0d.Id.Key = 0;
manifold._points[0] = p0d;
}
}
/// <summary>
/// This is one the methods of the IContactListener interface.
/// No implementation.
/// </summary>
/// <param name="contact">Box2D Contact</param>
/// <param name="oldManifold">Box2D Manifold</param>
public void PreSolve(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"></param>
/// <param name="xfA"></param>
/// <param name="radiusA"></param>
/// <param name="xfB"></param>
/// <param name="radiusB"></param>
public WorldManifold(ref Manifold manifold,
ref Transform xfA, float radiusA,
ref Transform xfB, 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 xfA, manifold._localPoint);
Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
_normal = new Vector2(1.0f, 0.0f);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_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 xfA.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(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.Multiply(ref xfB.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfA, 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;
}
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public static void CollideEdgeAndCircle(ref Manifold manifold,
EdgeShape edgeA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold._pointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MultiplyT(ref xfA, MathUtils.Multiply(ref xfB, circleB._p));
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(d, d);
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;
var mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB._p;
manifold._points[0] = mp;
return;
}
// Region B
if (u <= 0.0f)
{
P = B;
d = Q - P;
float dd = Vector2.Dot(d, d);
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;
var mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB._p;
manifold._points[0] = mp;
return;
}
// Region AB
float den = Vector2.Dot(e, e);
Debug.Assert(den > 0.0f);
P = (1.0f / den) * (u * A + v * B);
d = Q - P;
float dd2 = Vector2.Dot(d, d);
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;
var mp2 = new ManifoldPoint();
mp2.Id.Key = 0;
mp2.Id.Features = cf;
mp2.LocalPoint = circleB._p;
manifold._points[0] = mp2;
}
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._vertexCount = polygonB_in._vertexCount;
s_polygonB._centroid = MathUtils.Multiply(ref xf, polygonB_in._centroid);
for (int i = 0; i < s_polygonB._vertexCount; ++i)
{
s_polygonB._vertices[i] = MathUtils.Multiply(ref xf, polygonB_in._vertices[i]);
s_polygonB._normals[i] = 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(v1, v2, edgeNormal, s_polygonB, totalRadius);
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.b2_linearSlop)
{
types[0] = EdgeType.Concave;
}
else if (s >= -0.1f * Settings.b2_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.b2_linearSlop)
{
types[1] = EdgeType.Concave;
}
else if (s >= -0.1f * Settings.b2_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(v0, v1, n0, s_polygonB, totalRadius);
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(v2, v3, n2, s_polygonB, totalRadius);
if (axis2.separation > edgeAxis.separation)
{
// The polygon should collide with the next edge
return;
}
}
EPAxis polygonAxis = ComputePolygonSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
if (polygonAxis.separation > totalRadius)
{
return;
}
// Use hysteresis for jitter reduction.
float k_relativeTol = 0.98f;
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._vertexCount;
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;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < Settings.b2_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.b2_maxManifoldPoints; ++i)
{
float separation;
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;
}
/// Evaluate this contact with your own manifold and transforms.
internal void Evaluate(ref Manifold manifold, ref Transform xfA, ref Transform xfB)
{
switch (_type)
{
case ContactType.Polygon:
Collision.CollidePolygons(ref manifold,
(PolygonShape)_fixtureA.GetShape(), ref xfA,
(PolygonShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.PolygonAndCircle:
Collision.CollidePolygonAndCircle(ref manifold,
(PolygonShape)_fixtureA.GetShape(), ref xfA,
(CircleShape) _fixtureB.GetShape(), ref xfB);
break;
case ContactType.EdgeAndCircle:
Collision.CollideEdgeAndCircle(ref manifold,
(EdgeShape) _fixtureA.GetShape(), ref xfA,
(CircleShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.EdgeAndPolygon:
Collision.CollideEdgeAndPolygon(ref manifold,
(EdgeShape) _fixtureA.GetShape(), ref xfA,
(PolygonShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.LoopAndCircle:
var loop = (LoopShape)_fixtureA.GetShape();
loop.GetChildEdge(ref s_edge, _indexA);
Collision.CollideEdgeAndCircle(ref manifold, s_edge, ref xfA,
(CircleShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.LoopAndPolygon:
var loop2 = (LoopShape)_fixtureA.GetShape();
loop2.GetChildEdge(ref s_edge, _indexA);
Collision.CollideEdgeAndPolygon(ref manifold, s_edge, ref xfA,
(PolygonShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.Circle:
Collision.CollideCircles(ref manifold,
(CircleShape)_fixtureA.GetShape(), ref xfA,
(CircleShape)_fixtureB.GetShape(), ref xfB);
break;
}
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygonA"></param>
/// <param name="xfA"></param>
/// <param name="circleB"></param>
/// <param name="xfB"></param>
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygonA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold._pointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Multiply(ref xfB, circleB._p);
Vector2 cLocal = MathUtils.MultiplyT(ref xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.b2_maxFloat;
float radius = polygonA._radius + circleB._radius;
int vertexCount = polygonA._vertexCount;
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.b2_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._p;
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._p;
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._p;
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._p;
p0d.Id.Key = 0;
manifold._points[0] = p0d;
}
}
public virtual void PreSolve(Contact contact, ref Manifold oldManifold)
{
// call this in the base class if points are needed
// PreSolveCalcPoints(contact, ref oldManifold);
}
protected void PreSolveCalcPoints(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>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold"></param>
/// <param name="xfA"></param>
/// <param name="xfB"></param>
internal void Evaluate(ref Manifold manifold, ref Transform xfA, ref Transform xfB)
{
switch (_type)
{
case ContactType.Polygon:
Collision.CollidePolygons(ref manifold,
(PolygonShape)_fixtureA.GetShape(), ref xfA,
(PolygonShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.PolygonAndCircle:
Collision.CollidePolygonAndCircle(ref manifold,
(PolygonShape)_fixtureA.GetShape(), ref xfA,
(CircleShape)_fixtureB.GetShape(), ref xfB);
break;
case ContactType.Circle:
Collision.CollideCircles(ref manifold,
(CircleShape)_fixtureA.GetShape(), ref xfA,
(CircleShape)_fixtureB.GetShape(), ref xfB);
break;
}
}
public void PreSolve(Contact contact, ref Manifold oldManifold)
{
var unitA = (Unit) contact.GetFixtureA().GetBody().GetUserData();
var unitB = (Unit) contact.GetFixtureB().GetBody().GetUserData();
if (!(unitA.EnableCollision && unitB.EnableCollision
&& GroupManager.Instance.ShouldContact(unitA.Group, unitB.Group)))
{
contact.SetEnabled(false);
}
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polyA"></param>
/// <param name="xfA"></param>
/// <param name="polyB"></param>
/// <param name="xfB"></param>
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, ref Transform xfA,
PolygonShape polyB, ref Transform xfB)
{
manifold._pointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
byte 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 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold._type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold._type = ManifoldType.FaceA;
flip = 0;
}
FixedArray2 <ClipVertex> incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1._vertexCount;
Vector2 v11 = poly1._vertices[edge1];
Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0];
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);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
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.b2_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;
cp.Id.Features.Flip = flip;
manifold._points[pointCount] = cp;
++pointCount;
}
}
manifold._pointCount = pointCount;
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polyA"></param>
/// <param name="xfA"></param>
/// <param name="polyB"></param>
/// <param name="xfB"></param>
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, ref Transform xfA,
PolygonShape polyB, ref Transform xfB)
{
manifold._pointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
byte 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 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold._type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold._type = ManifoldType.FaceA;
flip = 0;
}
FixedArray2<ClipVertex> incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1._vertexCount;
Vector2 v11 = poly1._vertices[edge1];
Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0];
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);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
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.b2_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;
cp.Id.Features.Flip = flip;
manifold._points[pointCount] = cp;
++pointCount;
}
}
manifold._pointCount = pointCount;
}
/// Get the contact manifold. Do not modify the manifold unless you understand the
/// internals of Box2D.
public void GetManifold(out Manifold manifold)
{
manifold = _manifold;
}
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>
/// 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"></param>
/// <param name="xfA"></param>
/// <param name="radiusA"></param>
/// <param name="xfB"></param>
/// <param name="radiusB"></param>
public WorldManifold(ref Manifold manifold,
ref Transform xfA, float radiusA,
ref Transform xfB, 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 xfA, manifold._localPoint);
Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
_normal = new Vector2(1.0f, 0.0f);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_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 xfA.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(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.Multiply(ref xfB.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfA, 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;
}
}
