protected override void Draw(Settings settings)
{
base.Draw(settings);
b2Manifold manifold = new b2Manifold();
b2Collision.b2CollidePolygons(manifold, m_polygonA, ref m_transformA, m_polygonB, ref m_transformB);
b2WorldManifold worldManifold = new b2WorldManifold();
worldManifold.Initialize(manifold, ref m_transformA, m_polygonA.Radius, ref m_transformB, m_polygonB.Radius);
m_debugDraw.DrawString(5, m_textLine, "point count = {0}", manifold.pointCount);
m_textLine += 15;
{
b2Color color = new b2Color(0.9f, 0.9f, 0.9f);
b2Vec2[] v = new b2Vec2[b2Settings.b2_maxPolygonVertices];
for (int i = 0; i < m_polygonA.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformA, m_polygonA.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.VertexCount, color);
for (int i = 0; i < m_polygonB.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformB, m_polygonB.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.VertexCount, color);
}
for (int i = 0; i < manifold.pointCount; ++i)
{
m_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, new b2Color(0.9f, 0.3f, 0.3f));
}
}
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
{
b2ChainShape chain = (b2ChainShape)m_fixtureA.Shape;
b2EdgeShape edge;
edge = chain.GetChildEdge(m_indexA);
b2Collision.b2CollideEdgeAndCircle(ref manifold, edge, ref xfA,
(b2CircleShape)m_fixtureB.Shape, ref xfB);
}
/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
public static void b2GetPointStates(b2PointState[] state1, b2PointState[] state2,
ref b2Manifold manifold1, ref b2Manifold manifold2)
{
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
state1[i] = b2PointState.b2_nullState;
state2[i] = b2PointState.b2_nullState;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.pointCount; ++i)
{
b2ContactFeature id = manifold1.points[i].id;
state1[i] = b2PointState.b2_removeState;
for (int j = 0; j < manifold2.pointCount; ++j)
{
if (manifold2.points[j].id.Equals(id))
{
state1[i] = b2PointState.b2_persistState;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.pointCount; ++i)
{
b2ContactFeature id = manifold2.points[i].id;
state2[i] = b2PointState.b2_addState;
for (int j = 0; j < manifold1.pointCount; ++j)
{
if (manifold1.points[j].id.Equals(id))
{
state2[i] = b2PointState.b2_persistState;
break;
}
}
}
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
base.PreSolve(contact, oldManifold);
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA != m_platform && fixtureA != m_character)
{
return;
}
if (fixtureB != m_platform && fixtureB != m_character)
{
return;
}
b2Vec2 position = m_character.Body.Position;
if (position.y < m_top + m_radius - 3.0f * b2Settings.b2_linearSlop)
{
contact.SetEnabled(false);
}
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndPolygon(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
b2EPCollider b = new b2EPCollider();
b.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));
b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float u = b2Math.b2Dot(e, B - Q);
float v = b2Math.b2Dot(e, Q - A);
float radius = edgeA.Radius + circleB.Radius;
b2ContactFeature cf = b2ContactFeature.Zero;
cf.indexB = 0;
cf.typeB = b2ContactFeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float dd = b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
b2Vec2 A1 = edgeA.Vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float u1 = b2Math.b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float dd = b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
b2Vec2 B2 = edgeA.Vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float v2 = b2Math.b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region AB
float den = b2Math.b2Dot(e, e);
System.Diagnostics.Debug.Assert(den > 0.0f);
b2Vec2 xP = (1.0f / den) * (u * A + v * B);
b2Vec2 xd = Q - xP;
float xdd = b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = new b2Vec2(-e.y, e.x);
if (b2Math.b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_face;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
}
/// Compute the collision manifold between two polygons.
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform 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 = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = tangent.UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(ref normal, ref v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)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 < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public void CopyFrom(b2Manifold other)
{
localNormal = other.localNormal;
localPoint = other.localPoint;
type = other.type;
pointCount = other.pointCount;
for (int i = 0; i < points.Length; i++)
{
var cp1 = points[i];
var cp2 = other.points[i];
cp1.id = cp2.id;
cp1.localPoint = cp2.localPoint;
cp1.normalImpulse = cp2.normalImpulse;
cp1.tangentImpulse = cp2.tangentImpulse;
}
//Array.Copy(other.points, points, points.Length);
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
}
/// Compute the collision manifold between two circles.
public static void b2CollideCircles(b2Manifold manifold, b2CircleShape circleA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float pAx = (xfA.q.c * circleA.Position.x - xfA.q.s * circleA.Position.y) + xfA.p.x;
float pAy = (xfA.q.s * circleA.Position.x + xfA.q.c * circleA.Position.y) + xfA.p.y;
float pBx = (xfB.q.c * circleB.Position.x - xfB.q.s * circleB.Position.y) + xfB.p.x;
float pBy = (xfB.q.s * circleB.Position.x + xfB.q.c * circleB.Position.y) + xfB.p.y;
float dx = pBx - pAx;
float dy = pBy - pAy;
float distSqr = dx * dx + dy * dy;
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.type = b2ManifoldType.e_circles;
manifold.localPoint = circleA.Position;
manifold.localNormal = b2Vec2.Zero;
manifold.pointCount = 1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
public static void b2CollidePolygons(b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform 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 = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = _incidentEdge;
b2FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent;
localTangent.x = v12.x - v11.x;
localTangent.y = v12.y - v11.y;
localTangent.Normalize();
b2Vec2 localNormal;
localNormal.x = localTangent.y; //.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
localNormal.y = -localTangent.x;
b2Vec2 planePoint;
planePoint.x = 0.5f * (v11.x + v12.x);
planePoint.y = 0.5f * (v11.y + v12.y);
b2Vec2 tangent;
tangent.x = xf1.q.c * localTangent.x - xf1.q.s * localTangent.y;
tangent.y = xf1.q.s * localTangent.x + xf1.q.c * localTangent.y;
float normalx = tangent.y; //UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
float normaly = -tangent.x;
float v11x = (xf1.q.c * v11.x - xf1.q.s * v11.y) + xf1.p.x;
float v11y = (xf1.q.s * v11.x + xf1.q.c * v11.y) + xf1.p.y;
float v12x = (xf1.q.c * v12.x - xf1.q.s * v12.y) + xf1.p.x;
float v12y = (xf1.q.s * v12.x + xf1.q.c * v12.y) + xf1.p.y;
// Face offset.
float frontOffset = normalx * v11x + normaly * v11y;
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -(tangent.x * v11x + tangent.y * v11y) + totalRadius;
float sideOffset2 = tangent.x * v12x + tangent.y * v12y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = _clipPoints1;
int np;
// Clip to box side 1
b2Vec2 t;
t.x = -tangent.x;
t.y = -tangent.y;
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, ref t, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
b2ClipVertex[] clipPoints2 = _clipPoints2;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref tangent, sideOffset2, (byte)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 < b2Settings.b2_maxManifoldPoints; ++i)
{
var v = clipPoints2[i].v;
//float separation = b2Math.b2Dot(ref normal, ref v) - frontOffset;
float separation = normalx * v.x + normaly * v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
//cp.localPoint = b2Math.b2MulT(ref xf2, ref v);
float px = v.x - xf2.p.x;
float py = v.y - xf2.p.y;
cp.localPoint.x = (xf2.q.c * px + xf2.q.s * py);
cp.localPoint.y = (-xf2.q.s * px + xf2.q.c * py);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
/// Compute the collision manifold between two circles.
public static void b2CollideCircles(ref b2Manifold manifold,
b2CircleShape circleA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
b2Vec2 pA = b2Math.b2Mul(xfA, circleA.Position);
b2Vec2 pB = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 d;// = pB - pA;
d.x = pB.x - pA.x;
d.y = pB.y - pA.y;
float distSqr = d.LengthSquared;
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.type = b2ManifoldType.e_circles;
manifold.localPoint = circleA.Position;
manifold.localNormal.SetZero();
manifold.pointCount = 1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
/// 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.
public void Initialize(b2Manifold[] manifold,
b2Transform xfA, float radiusA,
b2Transform xfB, float radiusB)
{
points = new b2Vec2[b2Settings.b2_maxManifoldPoints];
}
// The normal points from 1 to 2
static public void CollidePolygons(b2Manifold manifold,
b2PolygonShape polyA, b2Transform xfA,
b2PolygonShape polyB, b2Transform xfB)
{
ClipVertex cv;
manifold.m_pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
int edgeA = 0;
s_edgeAO[0] = edgeA;
float separationA = FindMaxSeparation(s_edgeAO, polyA, xfA, polyB, xfB);
edgeA = s_edgeAO[0];
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
s_edgeBO[0] = edgeB;
float separationB = FindMaxSeparation(s_edgeBO, polyB, xfB, polyA, xfA);
edgeB = s_edgeBO[0];
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference poly
b2PolygonShape poly2; // incident poly
b2Transform xf1;
b2Transform xf2;
int edge1; // reference edge
uint flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2Mat22 tMat;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.m_type = b2Manifold.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.m_type = b2Manifold.e_faceA;
flip = 0;
}
ClipVertex[] incidentEdge = s_incidentEdge;
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
List <b2Vec2> vertices1 = poly1.m_vertices;
b2Vec2 local_v11 = vertices1[edge1];
b2Vec2 local_v12;
if (edge1 + 1 < count1)
{
local_v12 = vertices1[(int)(edge1 + 1)];
}
else
{
local_v12 = vertices1[0];
}
b2Vec2 localTangent = s_localTangent;
localTangent.Set(local_v12.x - local_v11.x, local_v12.y - local_v11.y);
localTangent.Normalize();
b2Vec2 localNormal = s_localNormal;
localNormal.x = localTangent.y;
localNormal.y = -localTangent.x;
b2Vec2 planePoint = s_planePoint;
planePoint.Set(0.5f * (local_v11.x + local_v12.x), 0.5f * (local_v11.y + local_v12.y));
b2Vec2 tangent = s_tangent;
//tangent = b2Math.b2MulMV(xf1.R, localTangent);
tMat = xf1.R;
tangent.x = (tMat.col1.x * localTangent.x + tMat.col2.x * localTangent.y);
tangent.y = (tMat.col1.y * localTangent.x + tMat.col2.y * localTangent.y);
b2Vec2 tangent2 = s_tangent2;
tangent2.x = -tangent.x;
tangent2.y = -tangent.y;
b2Vec2 normal = s_normal;
normal.x = tangent.y;
normal.y = -tangent.x;
//v11 = b2Math.MulX(xf1, local_v11);
//v12 = b2Math.MulX(xf1, local_v12);
b2Vec2 v11 = s_v11;
b2Vec2 v12 = s_v12;
v11.x = xf1.position.x + (tMat.col1.x * local_v11.x + tMat.col2.x * local_v11.y);
v11.y = xf1.position.y + (tMat.col1.y * local_v11.x + tMat.col2.y * local_v11.y);
v12.x = xf1.position.x + (tMat.col1.x * local_v12.x + tMat.col2.x * local_v12.y);
v12.y = xf1.position.y + (tMat.col1.y * local_v12.x + tMat.col2.y * local_v12.y);
// Face offset
float frontOffset = normal.x * v11.x + normal.y * 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.
ClipVertex[] clipPoints1 = s_clipPoints1;
ClipVertex[] clipPoints2 = s_clipPoints2;
int np;
// Clip to box side 1
//np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
np = ClipSegmentToLine(clipPoints1, incidentEdge, tangent2, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.m_localPlaneNormal.SetV(localNormal);
manifold.m_localPoint.SetV(planePoint);
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
cv = clipPoints2[i];
float separation = normal.x * cv.v.x + normal.y * cv.v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.m_points[pointCount];
//cp.m_localPoint = b2Math.b2MulXT(xf2, cv.v);
tMat = xf2.R;
float tX = cv.v.x - xf2.position.x;
float tY = cv.v.y - xf2.position.y;
cp.m_localPoint.x = (tX * tMat.col1.x + tY * tMat.col1.y);
cp.m_localPoint.y = (tX * tMat.col2.x + tY * tMat.col2.y);
cp.m_id.Set(cv.id);
cp.m_id.features.flip = (int)flip;
++pointCount;
}
}
manifold.m_pointCount = pointCount;
}
static public void CollidePolygonAndCircle(
b2Manifold manifold,
b2PolygonShape polygon, b2Transform xf1,
b2CircleShape circle, b2Transform xf2)
{
manifold.m_pointCount = 0;
b2ManifoldPoint tPoint;
float dX;
float dY;
float positionX;
float positionY;
b2Vec2 tVec;
b2Mat22 tMat;
// Compute circle position in the frame of the polygon.
//b2Vec2 c = b2Mul(xf2, circle->m_localPosition);
tMat = xf2.R;
tVec = circle.m_p;
float cX = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
float cY = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
//b2Vec2 cLocal = b2MulT(xf1, c);
dX = cX - xf1.position.x;
dY = cY - xf1.position.y;
tMat = xf1.R;
float cLocalX = (dX * tMat.col1.x + dY * tMat.col1.y);
float cLocalY = (dX * tMat.col2.x + dY * tMat.col2.y);
float dist;
// Find the min separating edge.
int normalIndex = 0;
float separation = -float.MaxValue;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_vertexCount;
List <b2Vec2> vertices = polygon.m_vertices;
List <b2Vec2> normals = polygon.m_normals;
for (int i = 0; i < vertexCount; ++i)
{
//float32 s = b2Dot(normals[i], cLocal - vertices[i]);
tVec = vertices[i];
dX = cLocalX - tVec.x;
dY = cLocalY - tVec.y;
tVec = normals[i];
float s = tVec.x * dX + tVec.y * dY;
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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < float.MinValue)
{
manifold.m_pointCount = 1;
manifold.m_type = b2Manifold.e_faceA;
manifold.m_localPlaneNormal.SetV(normals[normalIndex]);
manifold.m_localPoint.x = 0.5f * (v1.x + v2.x);
manifold.m_localPoint.y = 0.5f * (v1.y + v2.y);
manifold.m_points[0].m_localPoint.SetV(circle.m_p);
manifold.m_points[0].m_id.key = 0;
return;
}
// Project the circle center onto the edge segment.
float u1 = (cLocalX - v1.x) * (v2.x - v1.x) + (cLocalY - v1.y) * (v2.y - v1.y);
float u2 = (cLocalX - v2.x) * (v1.x - v2.x) + (cLocalY - v2.y) * (v1.y - v2.y);
if (u1 <= 0.0f)
{
if ((cLocalX - v1.x) * (cLocalX - v1.x) + (cLocalY - v1.y) * (cLocalY - v1.y) > radius * radius)
{
return;
}
manifold.m_pointCount = 1;
manifold.m_type = b2Manifold.e_faceA;
manifold.m_localPlaneNormal.x = cLocalX - v1.x;
manifold.m_localPlaneNormal.y = cLocalY - v1.y;
manifold.m_localPlaneNormal.Normalize();
manifold.m_localPoint.SetV(v1);
manifold.m_points[0].m_localPoint.SetV(circle.m_p);
manifold.m_points[0].m_id.key = 0;
}
else if (u2 <= 0)
{
if ((cLocalX - v2.x) * (cLocalX - v2.x) + (cLocalY - v2.y) * (cLocalY - v2.y) > radius * radius)
{
return;
}
manifold.m_pointCount = 1;
manifold.m_type = b2Manifold.e_faceA;
manifold.m_localPlaneNormal.x = cLocalX - v2.x;
manifold.m_localPlaneNormal.y = cLocalY - v2.y;
manifold.m_localPlaneNormal.Normalize();
manifold.m_localPoint.SetV(v2);
manifold.m_points[0].m_localPoint.SetV(circle.m_p);
manifold.m_points[0].m_id.key = 0;
}
else
{
float faceCenterX = 0.5f * (v1.x + v2.x);
float faceCenterY = 0.5f * (v1.y + v2.y);
separation = (cLocalX - faceCenterX) * normals[vertIndex1].x + (cLocalY - faceCenterY) * normals[vertIndex1].y;
if (separation > radius)
{
return;
}
manifold.m_pointCount = 1;
manifold.m_type = b2Manifold.e_faceA;
manifold.m_localPlaneNormal.x = normals[vertIndex1].x;
manifold.m_localPlaneNormal.y = normals[vertIndex1].y;
manifold.m_localPlaneNormal.Normalize();
manifold.m_localPoint.Set(faceCenterX, faceCenterY);
manifold.m_points[0].m_localPoint.SetV(circle.m_p);
manifold.m_points[0].m_id.key = 0;
}
}
// 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
public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
m_xf = b2Math.b2MulT(xfA, xfB);
m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid);
m_v0 = edgeA.Vertex0;
m_v1 = edgeA.Vertex1;
m_v2 = edgeA.Vertex2;
m_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
b2Vec2 cenMinusV1 = m_centroidB - m_v1;
float offset1 = b2Math.b2Dot(ref m_normal1, ref cenMinusV1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Math.b2Cross(ref edge0, ref edge1) >= 0.0f;
b2Vec2 cenMinusV0 = m_centroidB - m_v0;
offset0 = b2Math.b2Dot(ref m_normal0, ref cenMinusV0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Math.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB = b2TempPolygon.Create();
m_polygonB.count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]);
m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]);
}
m_radius = 2.0f * b2Settings.b2_polygonRadius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxisType.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = new b2ClipVertex[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.type = b2ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)i1;
ie[0].id.typeA = b2ContactFeatureType.e_face;
ie[0].id.typeB = b2ContactFeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)i2;
ie[1].id.typeA = b2ContactFeatureType.e_face;
ie[1].id.typeB = b2ContactFeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)primaryAxis.index;
ie[0].id.typeA = b2ContactFeatureType.e_vertex;
ie[0].id.typeB = b2ContactFeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)primaryAxis.index;
ie[1].id.typeA = b2ContactFeatureType.e_vertex;
ie[1].id.typeB = b2ContactFeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxisType.e_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 < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void PreSolve(b2Contact contact, ref b2Manifold oldManifold)
{
//throw new NotImplementedException ();
}
// 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.
public void Initialize(b2Manifold manifold, ref b2Transform xfA, float radiusA, ref b2Transform xfB, float radiusB)
{
if (manifold.pointCount == 0)
{
normal = b2Vec2.Zero;
return;
}
switch (manifold.type)
{
case b2ManifoldType.e_circles:
{
#if false
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Math.b2Mul(ref xfA, ref manifold.localPoint);
b2Vec2 pointB = b2Math.b2Mul(ref xfB, ref manifold.points[0].localPoint);
if (b2Math.b2DistanceSquared(pointA, pointB) > b2Settings.b2_epsilonSqrd)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
#else
normal.x = 1.0f;
normal.y = 0.0f;
var localPoint = manifold.points[0].localPoint;
float pointAx = (xfA.q.c * manifold.localPoint.x - xfA.q.s * manifold.localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * manifold.localPoint.x + xfA.q.c * manifold.localPoint.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPoint.x - xfB.q.s * localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPoint.x + xfB.q.c * localPoint.y) + xfB.p.y;
float cx = pointAx - pointBx;
float cy = pointAy - pointBy;
float distance = (cx * cx + cy * cy);
if (distance > b2Settings.b2_epsilonSqrd)
{
normal.x = pointBx - pointAx;
normal.y = pointBy - pointAy;
normal.Normalize();
}
float cAx = pointAx + radiusA * normal.x;
float cAy = pointAy + radiusA * normal.y;
float cBx = pointBx - radiusB * normal.x;
float cBy = pointBy - radiusB * normal.y;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[0] = p;
#endif
}
break;
case b2ManifoldType.e_faceA:
{
#if false
normal = b2Math.b2Mul(xfA.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(ref xfA, ref manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(ref xfB, ref manifold.points[i].localPoint);
b2Vec2 clipMinusPlane = clipPoint - planePoint;
b2Vec2 cA = clipPoint + (radiusA - b2Math.b2Dot(ref clipMinusPlane, ref normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
#else
float normalx = xfA.q.c * manifold.localNormal.x - xfA.q.s * manifold.localNormal.y;
float normaly = xfA.q.s * manifold.localNormal.x + xfA.q.c * manifold.localNormal.y;
normal.x = normalx;
normal.y = normaly;
float planePointx = (xfA.q.c * manifold.localPoint.x - xfA.q.s * manifold.localPoint.y) + xfA.p.x;
float planePointy = (xfA.q.s * manifold.localPoint.x + xfA.q.c * manifold.localPoint.y) + xfA.p.y;
for (int i = 0; i < manifold.pointCount; ++i)
{
var localPoint = manifold.points[i].localPoint;
float clipPointx = (xfB.q.c * localPoint.x - xfB.q.s * localPoint.y) + xfB.p.x;
float clipPointy = (xfB.q.s * localPoint.x + xfB.q.c * localPoint.y) + xfB.p.y;
float clipMinusPlanex = clipPointx - planePointx;
float clipMinusPlaney = clipPointy - planePointy;
float d = clipMinusPlanex * normalx + clipMinusPlaney * normaly;
float cAx = clipPointx + (radiusA - d) * normalx;
float cAy = clipPointy + (radiusA - d) * normaly;
float cBx = clipPointx - radiusB * normalx;
float cBy = clipPointy - radiusB * normaly;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[i] = p;
}
#endif
}
break;
case b2ManifoldType.e_faceB:
{
#if false
normal = b2Math.b2Mul(ref xfB.q, ref manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(ref xfB, ref manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(ref xfA, ref manifold.points[i].localPoint);
b2Vec2 tmp = b2Vec2.Zero;
tmp.x = clipPoint.x - planePoint.x;
tmp.y = clipPoint.y - planePoint.y;
// b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(ref tmp, ref normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
#else
float normalx = xfB.q.c * manifold.localNormal.x - xfB.q.s * manifold.localNormal.y;
float normaly = xfB.q.s * manifold.localNormal.x + xfB.q.c * manifold.localNormal.y;
float planePointx = (xfB.q.c * manifold.localPoint.x - xfB.q.s * manifold.localPoint.y) + xfB.p.x;
float planePointy = (xfB.q.s * manifold.localPoint.x + xfB.q.c * manifold.localPoint.y) + xfB.p.y;
for (int i = 0; i < manifold.pointCount; ++i)
{
var localPoint = manifold.points[i].localPoint;
float clipPointx = (xfA.q.c * localPoint.x - xfA.q.s * localPoint.y) + xfA.p.x;
float clipPointy = (xfA.q.s * localPoint.x + xfA.q.c * localPoint.y) + xfA.p.y;
float distx = clipPointx - planePointx;
float disty = clipPointy - planePointy;
var d = (distx * normalx + disty * normaly);
float cBx = clipPointx + (radiusB - d) * normalx;
float cBy = clipPointy + (radiusB - d) * normaly;
float cAx = clipPointx - radiusA * normalx;
float cAy = clipPointy - radiusA * normaly;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[i] = p;
}
// Ensure normal points from A to B.
normal.x = -normalx;
normal.y = -normaly;
#endif
}
break;
}
}
public static void b2CollidePolygons(b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform 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 = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = _incidentEdge;
b2FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent;
localTangent.x = v12.x - v11.x;
localTangent.y = v12.y - v11.y;
localTangent.Normalize();
b2Vec2 localNormal;
localNormal.x = localTangent.y; //.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
localNormal.y = -localTangent.x;
b2Vec2 planePoint;
planePoint.x = 0.5f * (v11.x + v12.x);
planePoint.y = 0.5f * (v11.y + v12.y);
b2Vec2 tangent;
tangent.x = xf1.q.c * localTangent.x - xf1.q.s * localTangent.y;
tangent.y = xf1.q.s * localTangent.x + xf1.q.c * localTangent.y;
float normalx = tangent.y; //UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
float normaly = -tangent.x;
float v11x = (xf1.q.c * v11.x - xf1.q.s * v11.y) + xf1.p.x;
float v11y = (xf1.q.s * v11.x + xf1.q.c * v11.y) + xf1.p.y;
float v12x = (xf1.q.c * v12.x - xf1.q.s * v12.y) + xf1.p.x;
float v12y = (xf1.q.s * v12.x + xf1.q.c * v12.y) + xf1.p.y;
// Face offset.
float frontOffset = normalx * v11x + normaly * v11y;
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -(tangent.x * v11x + tangent.y * v11y) + totalRadius;
float sideOffset2 = tangent.x * v12x + tangent.y * v12y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = _clipPoints1;
int np;
// Clip to box side 1
b2Vec2 t;
t.x = -tangent.x;
t.y = -tangent.y;
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, ref t, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
b2ClipVertex[] clipPoints2 = _clipPoints2;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref tangent, sideOffset2, (byte)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 < b2Settings.b2_maxManifoldPoints; ++i)
{
var v = clipPoints2[i].v;
//float separation = b2Math.b2Dot(ref normal, ref v) - frontOffset;
float separation = normalx * v.x + normaly * v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
//cp.localPoint = b2Math.b2MulT(ref xf2, ref v);
float px = v.x - xf2.p.x;
float py = v.y - xf2.p.y;
cp.localPoint.x = (xf2.q.c * px + xf2.q.s * py);
cp.localPoint.y = (-xf2.q.s * px + xf2.q.c * py);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(ref b2Manifold manifold,
b2PolygonShape polygonA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.VertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(normals[i], cLocal - 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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
public virtual void SetManifold(b2Manifold m)
{
m_manifold = m;
}
// 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
public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
m_xf = b2Math.b2MulT(xfA, xfB);
m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid);
m_v0 = edgeA.Vertex0;
m_v1 = edgeA.Vertex1;
m_v2 = edgeA.Vertex2;
m_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float offset1 = b2Math.b2Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Math.b2Cross(edge0, edge1) >= 0.0f;
offset0 = b2Math.b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Math.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB = b2TempPolygon.Create();
m_polygonB.count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]);
m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]);
}
m_radius = 2.0f * b2Settings.b2_polygonRadius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxisType.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = new b2ClipVertex[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
manifold.type = b2ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)i1;
ie[0].id.typeA = b2ContactFeatureType.e_face;
ie[0].id.typeB = b2ContactFeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)i2;
ie[1].id.typeA = b2ContactFeatureType.e_face;
ie[1].id.typeB = b2ContactFeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.indexA = 0;
ie[0].id.indexB = (byte)primaryAxis.index;
ie[0].id.typeA = b2ContactFeatureType.e_vertex;
ie[0].id.typeB = b2ContactFeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.indexA = 0;
ie[1].id.indexB = (byte)primaryAxis.index;
ie[1].id.typeA = b2ContactFeatureType.e_vertex;
ie[1].id.typeB = b2ContactFeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxisType.e_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 < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxisType.e_edgeA)
{
cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(b2Manifold manifold, b2PolygonShape polygonA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c;
c.x = (xfB.q.c * circleB.Position.x - xfB.q.s * circleB.Position.y) + xfB.p.x;
c.y = (xfB.q.s * circleB.Position.x + xfB.q.c * circleB.Position.y) + xfB.p.y;
b2Vec2 cLocal;
float px = c.x - xfA.p.x;
float py = c.y - xfA.p.y;
cLocal.x = (xfA.q.c * px + xfA.q.s * py);
cLocal.y = (-xfA.q.s * px + xfA.q.c * py);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.m_vertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
b2Vec2 tmp;
tmp.x = cLocal.x - vertices[i].x;
tmp.y = cLocal.y - vertices[i].y;
float s = normals[i].x * tmp.x + normals[i].y * tmp.y; // b2Math.b2Dot(normals[i], cLocal - 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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint.x = 0.5f * (v1.x + v2.x);
manifold.localPoint.y = 0.5f * (v1.y + v2.y);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float ax = cLocal.x - v1.x;
float ay = cLocal.y - v1.y;
float bx = v2.x - v1.x;
float by = v2.y - v1.y;
float u1 = ax * bx + ay * by;
ax = cLocal.x - v2.x;
ay = cLocal.y - v2.y;
bx = v1.x - v2.x;
by = v1.y - v2.y;
float u2 = ax * bx + ay * by;
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal.x = cLocal.x - v2.x;
manifold.localNormal.y = cLocal.y - v2.y;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter;
faceCenter.x = 0.5f * (v1.x + v2.x);
faceCenter.y = 0.5f * (v1.y + v2.y);
b2Vec2 a;
a.x = cLocal.x - faceCenter.x;
a.y = cLocal.y - faceCenter.y;
separation = b2Math.b2Dot(ref a, ref normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
}
/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
public static void b2GetPointStates(b2PointState[] state1, b2PointState[] state2, b2Manifold manifold1, b2Manifold manifold2)
{
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
state1[i] = b2PointState.b2_nullState;
state2[i] = b2PointState.b2_nullState;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.pointCount; ++i)
{
b2ContactFeature id = manifold1.points[i].id;
state1[i] = b2PointState.b2_removeState;
for (int j = 0; j < manifold2.pointCount; ++j)
{
if (manifold2.points[j].id.Equals(ref id))
{
state1[i] = b2PointState.b2_persistState;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.pointCount; ++i)
{
b2ContactFeature id = manifold2.points[i].id;
state2[i] = b2PointState.b2_addState;
for (int j = 0; j < manifold1.pointCount; ++j)
{
if (manifold1.points[j].id.Equals(ref id))
{
state2[i] = b2PointState.b2_persistState;
break;
}
}
}
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndPolygon(b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2PolygonShape polygonB, ref b2Transform xfB)
{
b2EPCollider b = b2EPCollider.Create();
b.Collide(manifold, edgeA, ref xfA, polygonB, ref xfB);
b.Free();
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
b2Manifold manifold = contact.GetManifold();
if (manifold.pointCount == 0)
{
return;
}
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
b2Collision.b2GetPointStates(state1, state2, oldManifold, manifold);
contact.GetWorldManifold(ref worldManifold);
for (int i = 0; i < manifold.pointCount && m_pointCount < k_maxContactPoints; ++i)
{
ContactPoint cp = m_points[m_pointCount];
if (cp == null)
{
cp = new ContactPoint();
m_points[m_pointCount] = cp;
}
cp.fixtureA = fixtureA;
cp.fixtureB = fixtureB;
cp.position = worldManifold.points[i];
cp.normal = worldManifold.normal;
cp.state = state2[i];
++m_pointCount;
}
}
// 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.
public void Initialize(b2Manifold manifold, ref b2Transform xfA, float radiusA, ref b2Transform xfB, float radiusB)
{
if (manifold.pointCount == 0)
{
normal = b2Vec2.Zero;
return;
}
switch (manifold.type)
{
case b2ManifoldType.e_circles:
{
#if false
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Math.b2Mul(ref xfA, ref manifold.localPoint);
b2Vec2 pointB = b2Math.b2Mul(ref xfB, ref manifold.points[0].localPoint);
if (b2Math.b2DistanceSquared(pointA, pointB) > b2Settings.b2_epsilonSqrd)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
#else
normal.x = 1.0f;
normal.y = 0.0f;
var localPoint = manifold.points[0].localPoint;
float pointAx = (xfA.q.c * manifold.localPoint.x - xfA.q.s * manifold.localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * manifold.localPoint.x + xfA.q.c * manifold.localPoint.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPoint.x - xfB.q.s * localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPoint.x + xfB.q.c * localPoint.y) + xfB.p.y;
float cx = pointAx - pointBx;
float cy = pointAy - pointBy;
float distance = (cx * cx + cy * cy);
if (distance > b2Settings.b2_epsilonSqrd)
{
normal.x = pointBx - pointAx;
normal.y = pointBy - pointAy;
normal.Normalize();
}
float cAx = pointAx + radiusA * normal.x;
float cAy = pointAy + radiusA * normal.y;
float cBx = pointBx - radiusB * normal.x;
float cBy = pointBy - radiusB * normal.y;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[0] = p;
#endif
}
break;
case b2ManifoldType.e_faceA:
{
#if false
normal = b2Math.b2Mul(xfA.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(ref xfA, ref manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(ref xfB, ref manifold.points[i].localPoint);
b2Vec2 clipMinusPlane = clipPoint - planePoint;
b2Vec2 cA = clipPoint + (radiusA - b2Math.b2Dot(ref clipMinusPlane, ref normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
#else
float normalx = xfA.q.c * manifold.localNormal.x - xfA.q.s * manifold.localNormal.y;
float normaly = xfA.q.s * manifold.localNormal.x + xfA.q.c * manifold.localNormal.y;
normal.x = normalx;
normal.y = normaly;
float planePointx = (xfA.q.c * manifold.localPoint.x - xfA.q.s * manifold.localPoint.y) + xfA.p.x;
float planePointy = (xfA.q.s * manifold.localPoint.x + xfA.q.c * manifold.localPoint.y) + xfA.p.y;
for (int i = 0; i < manifold.pointCount; ++i)
{
var localPoint = manifold.points[i].localPoint;
float clipPointx = (xfB.q.c * localPoint.x - xfB.q.s * localPoint.y) + xfB.p.x;
float clipPointy = (xfB.q.s * localPoint.x + xfB.q.c * localPoint.y) + xfB.p.y;
float clipMinusPlanex = clipPointx - planePointx;
float clipMinusPlaney = clipPointy - planePointy;
float d = clipMinusPlanex * normalx + clipMinusPlaney * normaly;
float cAx = clipPointx + (radiusA - d) * normalx;
float cAy = clipPointy + (radiusA - d) * normaly;
float cBx = clipPointx - radiusB * normalx;
float cBy = clipPointy - radiusB * normaly;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[i] = p;
}
#endif
}
break;
case b2ManifoldType.e_faceB:
{
#if false
normal = b2Math.b2Mul(ref xfB.q, ref manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(ref xfB, ref manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(ref xfA, ref manifold.points[i].localPoint);
b2Vec2 tmp = b2Vec2.Zero;
tmp.x = clipPoint.x - planePoint.x;
tmp.y = clipPoint.y - planePoint.y;
// b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(ref tmp, ref normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
#else
float normalx = xfB.q.c * manifold.localNormal.x - xfB.q.s * manifold.localNormal.y;
float normaly = xfB.q.s * manifold.localNormal.x + xfB.q.c * manifold.localNormal.y;
float planePointx = (xfB.q.c * manifold.localPoint.x - xfB.q.s * manifold.localPoint.y) + xfB.p.x;
float planePointy = (xfB.q.s * manifold.localPoint.x + xfB.q.c * manifold.localPoint.y) + xfB.p.y;
for (int i = 0; i < manifold.pointCount; ++i)
{
var localPoint = manifold.points[i].localPoint;
float clipPointx = (xfA.q.c * localPoint.x - xfA.q.s * localPoint.y) + xfA.p.x;
float clipPointy = (xfA.q.s * localPoint.x + xfA.q.c * localPoint.y) + xfA.p.y;
float distx = clipPointx - planePointx;
float disty = clipPointy - planePointy;
var d = (distx * normalx + disty * normaly);
float cBx = clipPointx + (radiusB - d) * normalx;
float cBy = clipPointy + (radiusB - d) * normaly;
float cAx = clipPointx - radiusA * normalx;
float cAy = clipPointy - radiusA * normaly;
b2Vec2 p;
p.x = 0.5f * (cAx + cBx);
p.y = 0.5f * (cAy + cBy);
points[i] = p;
}
// Ensure normal points from A to B.
normal.x = -normalx;
normal.y = -normaly;
#endif
}
break;
}
}
/// Compute the collision manifold between two circles.
public static void b2CollideCircles(ref b2Manifold manifold,
b2CircleShape circleA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
b2Vec2 pA = b2Math.b2Mul(xfA, circleA.Position);
b2Vec2 pB = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 d = pB - pA;
float distSqr = b2Math.b2Dot(d, d);
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.type = b2ManifoldType.e_circles;
manifold.localPoint = circleA.Position;
manifold.localNormal.SetZero();
manifold.pointCount = 1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
public override void PreSolve(Box2D.Dynamics.Contacts.b2Contact contact, b2Manifold oldManifold)
{
}
/// Compute the collision manifold between two polygons.
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform 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 = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(tangent, v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)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 < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
{
b2Collision.b2CollidePolygons(ref manifold,
(b2PolygonShape)m_fixtureA.Shape, ref xfA,
(b2PolygonShape)m_fixtureB.Shape, ref xfB);
}
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
{
b2Collision.b2CollideEdgeAndCircle(ref manifold,
(b2EdgeShape)m_fixtureA.Shape, ref xfA,
(b2CircleShape)m_fixtureB.Shape, ref xfB);
}
/// <summary> /// This is called after a contact is updated. This allows you to inspect a /// contact before it goes to the solver. If you are careful, you can modify the /// contact manifold (e.g. disable contact). /// A copy of the old manifold is provided so that you can detect changes. /// Note: this is called only for awake bodies. /// Note: this is called even when the number of contact points is zero. /// Note: this is not called for sensors. /// Note: if you set the number of contact points to zero, you will not /// get an EndContact callback. However, you may get a BeginContact callback /// the next step. /// </summary> public abstract void PreSolve(b2Contact contact, b2Manifold oldManifold);
public b2Vec2[] points; //< world contact point (point of intersection)
#endregion Fields
#region Methods
// 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.
public void Initialize(ref b2Manifold manifold,
b2Transform xfA, float radiusA,
b2Transform xfB, float radiusB)
{
points = new b2Vec2[b2Settings.b2_maxManifoldPoints];
for (int p = 0; p < b2Settings.b2_maxManifoldPoints; p++)
points[p] = b2Vec2.Zero;
normal = b2Vec2.Zero;
if (manifold.pointCount == 0)
{
return;
}
switch (manifold.type)
{
case b2ManifoldType.e_circles:
{
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Math.b2Mul(xfA, manifold.localPoint);
b2Vec2 pointB = b2Math.b2Mul(xfB, manifold.points[0].localPoint);
if (b2Math.b2DistanceSquared(pointA, pointB) > b2Settings.b2_epsilonSqrd)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case b2ManifoldType.e_faceA:
{
normal = b2Math.b2Mul(xfA.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfA, manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(xfB, manifold.points[i].localPoint);
b2Vec2 cA = clipPoint + (radiusA - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case b2ManifoldType.e_faceB:
{
normal = b2Math.b2Mul(xfB.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfB, manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(xfA, manifold.points[i].localPoint);
b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(ref b2Manifold manifold,
b2PolygonShape polygonA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.VertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(normals[i], cLocal - 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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
public static b2Manifold Create()
{
b2Manifold m = new b2Manifold();
m.points = new b2ManifoldPoint[b2Settings.b2_maxManifoldPoints];
return (m);
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));
b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
b2Vec2 e = B - A;
b2Vec2 diff;
// Barycentric coordinates
diff = B - Q;
float u = b2Math.b2Dot(ref e, ref diff); // B - Q);
diff = Q - A;
float v = b2Math.b2Dot(ref e, ref diff); // Q - A);
float radius = edgeA.Radius + circleB.Radius;
b2ContactFeature cf = b2ContactFeature.Zero;
cf.indexB = 0;
cf.typeB = b2ContactFeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
b2Vec2 A1 = edgeA.Vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
diff = B1 - Q;
float u1 = b2Math.b2Dot(ref e1, ref diff);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
b2Vec2 B2 = edgeA.Vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
diff = Q - A2;
float v2 = b2Math.b2Dot(ref e2, ref diff);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region AB
float den = e.Length; // b2Math.b2Dot(e, e);
System.Diagnostics.Debug.Assert(den > 0.0f);
b2Vec2 xP = (1.0f / den) * (u * A + v * B);
b2Vec2 xd = Q - xP;
float xdd = xd.LengthSquared; // b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x);
n.m_x = -e.y;
n.m_y = e.x;
diff = Q - A;
if (b2Math.b2Dot(ref n, ref diff) < 0.0f)
{
// n.Set(-n.x, -n.y);
n.Set(-n.m_x, -n.m_y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_face;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
}
public b2Contact(b2Fixture fA, int indexA, b2Fixture fB, int indexB)
{
m_flags = b2ContactFlags.e_enabledFlag;
m_fixtureA = fA;
m_fixtureB = fB;
m_indexA = indexA;
m_indexB = indexB;
m_manifold = b2Manifold.Create();
m_manifold.pointCount = 0;
Prev = null;
Next = null;
m_NodeA = new b2ContactEdge();
m_NodeA.Contact = null;
m_NodeA.hasPrev = false;
m_NodeA.hasNext = false;
m_NodeA.Other = null;
m_nodeB = new b2ContactEdge();
m_nodeB.Contact = null;
m_nodeB.hasPrev = false;
m_nodeB.hasNext = false;
m_nodeB.Other = null;
m_toiCount = 0;
m_friction = b2Math.b2MixFriction(m_fixtureA.Friction, m_fixtureB.Friction);
m_restitution = b2Math.b2MixRestitution(m_fixtureA.Restitution, m_fixtureB.Restitution);
}
/// Evaluate this contact with your own manifold and transforms. public abstract void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB);
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(b2Manifold manifold, b2PolygonShape polygonA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c;
c.x = (xfB.q.c * circleB.Position.x - xfB.q.s * circleB.Position.y) + xfB.p.x;
c.y = (xfB.q.s * circleB.Position.x + xfB.q.c * circleB.Position.y) + xfB.p.y;
b2Vec2 cLocal;
float px = c.x - xfA.p.x;
float py = c.y - xfA.p.y;
cLocal.x = (xfA.q.c * px + xfA.q.s * py);
cLocal.y = (-xfA.q.s * px + xfA.q.c * py);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.m_vertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
b2Vec2 tmp;
tmp.x = cLocal.x - vertices[i].x;
tmp.y = cLocal.y - vertices[i].y;
float s = normals[i].x * tmp.x + normals[i].y * tmp.y; // b2Math.b2Dot(normals[i], cLocal - 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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint.x = 0.5f * (v1.x + v2.x);
manifold.localPoint.y = 0.5f * (v1.y + v2.y);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float ax = cLocal.x - v1.x;
float ay = cLocal.y - v1.y;
float bx = v2.x - v1.x;
float by = v2.y - v1.y;
float u1 = ax * bx + ay * by;
ax = cLocal.x - v2.x;
ay = cLocal.y - v2.y;
bx = v1.x - v2.x;
by = v1.y - v2.y;
float u2 = ax * bx + ay * by;
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal.x = cLocal.x - v2.x;
manifold.localNormal.y = cLocal.y - v2.y;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter;
faceCenter.x = 0.5f * (v1.x + v2.x);
faceCenter.y = 0.5f * (v1.y + v2.y);
b2Vec2 a;
a.x = cLocal.x - faceCenter.x;
a.y = cLocal.y - faceCenter.y;
separation = b2Math.b2Dot(ref a, ref normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
}