public Manifold()
{
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
{
Points[i] = new ManifoldPoint();
}
}
public ManifoldPoint Clone()
{
ManifoldPoint newPoint = new ManifoldPoint();
newPoint.LocalPoint = this.LocalPoint;
newPoint.NormalImpulse = this.NormalImpulse;
newPoint.TangentImpulse = this.TangentImpulse;
newPoint.ID = this.ID;
return(newPoint);
}
public ManifoldPoint Clone()
{
ManifoldPoint newPoint = new ManifoldPoint();
newPoint.LocalPoint1 = this.LocalPoint1;
newPoint.LocalPoint2 = this.LocalPoint2;
newPoint.Separation = this.Separation;
newPoint.NormalImpulse = this.NormalImpulse;
newPoint.TangentImpulse = this.TangentImpulse;
newPoint.ID = this.ID;
return(newPoint);
}
public Manifold Clone()
{
Manifold manifold = new Manifold();
manifold.Normal = this.Normal;
manifold.PointCount = this.PointCount;
int num = this.Points.Length;
ManifoldPoint[] array = new ManifoldPoint[num];
for (int i = 0; i < num; i++)
{
array[i] = this.Points[i].Clone();
}
manifold.Points = array;
return manifold;
}
public Manifold Clone()
{
Manifold newManifold = new Manifold();
newManifold.Normal = this.Normal;
newManifold.PointCount = this.PointCount;
int pointCount = this.Points.Length;
ManifoldPoint[] tmp = new ManifoldPoint[pointCount];
for (int i = 0; i < pointCount; i++)
{
tmp[i] = this.Points[i].Clone();
}
newManifold.Points = tmp;
return(newManifold);
}
public Manifold Clone()
{
Manifold manifold = new Manifold();
manifold.Normal = this.Normal;
manifold.PointCount = this.PointCount;
int num = this.Points.Length;
ManifoldPoint[] array = new ManifoldPoint[num];
for (int i = 0; i < num; i++)
{
array[i] = this.Points[i].Clone();
}
manifold.Points = array;
return(manifold);
}
public Manifold()
{
for (int i = 0; i < Settings.MaxManifoldPoints; i++)
Points[i] = new ManifoldPoint();
}
public Manifold Clone()
{
Manifold newManifold = new Manifold();
newManifold.LocalPlaneNormal = this.LocalPlaneNormal;
newManifold.LocalPoint = this.LocalPoint;
newManifold.Type = this.Type;
newManifold.PointCount = this.PointCount;
int pointCount = this.Points.Length;
ManifoldPoint[] tmp = new ManifoldPoint[pointCount];
for (int i = 0; i < pointCount; i++)
{
tmp[i] = this.Points[i].Clone();
}
newManifold.Points = tmp;
return newManifold;
}
public ManifoldPoint Clone()
{
ManifoldPoint newPoint = new ManifoldPoint();
newPoint.LocalPoint = this.LocalPoint;
newPoint.NormalImpulse = this.NormalImpulse;
newPoint.TangentImpulse = this.TangentImpulse;
newPoint.ID = this.ID;
return newPoint;
}
// 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 CollidePolygons(ref Manifold manifold,
PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
{
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1._vertexCount;
Vector2[] vertices1 = poly1._vertices;
Vector2 v11 = vertices1[edge1];
Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vector2 dv = v12 - v11;
Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f);
localNormal.Normalize();
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 sideNormal = xf1.TransformDirection(v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public ManifoldPoint Clone()
{
ManifoldPoint newPoint = new ManifoldPoint();
newPoint.LocalPoint1 = this.LocalPoint1;
newPoint.LocalPoint2 = this.LocalPoint2;
newPoint.Separation = this.Separation;
newPoint.NormalImpulse = this.NormalImpulse;
newPoint.TangentImpulse = this.TangentImpulse;
newPoint.ID = this.ID;
return newPoint;
}
// 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 CollidePolygons(ref Manifold manifold,
PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
{
manifold.PointCount = 0;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.GetVertices();
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 dv = v12 - v11;
Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vec2.Dot(frontNormal, v11);
float sideOffset1 = -Vec2.Dot(sideNormal, v11);
float sideOffset2 = Vec2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.Normal = flip != 0 ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= 0.0f)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.Separation = separation;
cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
// 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 CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB)
{
manifold = new Manifold();
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Vec2.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Math.Mul(xf1.R, localTangent);
Vec2 normal = Vec2.Cross(tangent, 1.0f);
v11 = Math.Mul(xf1, v11);
v12 = Math.Mul(xf1, v12);
// Face offset.
float frontOffset = Vec2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
