public void Set(b2ManifoldPoint m)
{
m_localPoint.SetV(m.m_localPoint);
m_normalImpulse = m.m_normalImpulse;
m_tangentImpulse = m.m_tangentImpulse;
m_id.Set(m.m_id);
}
public int pointCount; //< the number of manifold points
public b2Manifold()
{
points = new b2ManifoldPoint[b2Settings.b2_maxManifoldPoints];
for (int i = 0; i < points.Length; i++)
{
points[i] = new b2ManifoldPoint();
}
}
public b2ManifoldPoint[] CopyPoints()
{
b2ManifoldPoint[] copy = new b2ManifoldPoint[points.Length];
points.CopyTo(copy, 0);
return(copy);
}
/// 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;
}
// 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(b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2PolygonShape polygonB, ref b2Transform xfB)
{
m_xf = b2Math.b2MulT(xfA, xfB);
m_centroidB = b2Math.b2Mul(ref m_xf, ref 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.x = m_v2.x - m_v1.x;
edge1.y = m_v2.y - m_v1.y;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
b2Vec2 cenMinusV1;// = m_centroidB - m_v1;
cenMinusV1.x = m_centroidB.x - m_v1.x;
cenMinusV1.y = m_centroidB.y - m_v1.y;
float offset1 = m_normal1.x * cenMinusV1.x + m_normal1.y * cenMinusV1.y;// 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.x = m_v1.x - m_v0.x;
edge0.y = m_v1.y - m_v0.y;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Math.b2Cross(ref edge0, ref edge1) >= 0.0f;
b2Vec2 cenMinusV0;// = m_centroidB - m_v0;
cenMinusV0.x = m_centroidB.x - m_v0.x;
cenMinusV0.y = m_centroidB.y - m_v0.y;
offset0 = m_normal0.x * cenMinusV0.x + m_normal0.y * cenMinusV0.y; // b2Math.b2Dot(ref m_normal0, ref cenMinusV0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2;// = m_v3 - m_v2;
edge2.x = m_v3.x - m_v2.x;
edge2.y = m_v3.y - m_v2.y;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Math.b2Cross(ref edge1, ref edge2) > 0.0f;
b2Vec2 tmp;
tmp.x = m_centroidB.x - m_v2.x;
tmp.y = m_centroidB.y - m_v2.y;
offset2 = m_normal2.x * tmp.x + m_normal2.y * tmp.y;// 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.count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
m_polygonB.vertices[i] = b2Math.b2Mul(ref m_xf, ref polygonB.Vertices[i]);
m_polygonB.normals[i] = b2Math.b2Mul(ref m_xf.q, ref 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;
}
//Cached b2ClipVertex[] ie = new b2ClipVertex[2];
b2ClipVertex[] ie = _ie;
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(ref m_normal, ref m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = b2Math.b2Dot(ref m_normal, ref 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(ref rf.sideNormal1, ref rf.v1);
rf.sideOffset2 = b2Math.b2Dot(ref rf.sideNormal2, ref rf.v2);
// Clip incident edge against extruded edge1 side edges.
//Cached b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
//Cached b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints1 = _clipPoints1;
b2ClipVertex[] clipPoints2 = _clipPoints2;
int np;
// Clip to box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, ref rf.sideNormal1, rf.sideOffset1, (byte)rf.i1);
if (np < b2Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, ref 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(ref m_xf, ref 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;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public int pointCount; //< the number of manifold points
public b2Manifold()
{
points = new b2ManifoldPoint[b2Settings.b2_maxManifoldPoints];
for (int i = 0; i < points.Length; i++)
{
points[i] = new b2ManifoldPoint();
}
}
public b2ManifoldPoint[] CopyPoints()
{
b2ManifoldPoint[] copy = new b2ManifoldPoint[points.Length];
points.CopyTo(copy, 0);
return (copy);
}
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;
}
// 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;
}
