public override void Step(Settings settings)
{
settings.pause = 1;
base.Step(settings);
settings.pause = 0;
Sweep sweep1 = new Sweep();
sweep1.C0.Set(0.0f, 20.0f);
sweep1.A0 = 0.0f;
sweep1.C = sweep1.C0;
sweep1.A = sweep1.A0;
sweep1.T0 = 0.0f;
sweep1.LocalCenter = _body1.GetLocalCenter();
Sweep sweep2 = new Sweep();
sweep2.C0.Set(9.6363468f, 28.050615f);
sweep2.A0 = 1.6408679f;
sweep2.C = sweep2.C0 + new Vec2(-0.075121880f, 0.27358246f);
sweep2.A = sweep2.A0 - 10.434675f;
sweep2.T0 = 0.0f;
sweep2.LocalCenter = _body2.GetLocalCenter();
float toi = Collision.TimeOfImpact(_shape1, sweep1, _shape2, sweep2);
OpenGLDebugDraw.DrawString(5, _textLine, "toi = " + toi.ToString());
_textLine += 15;
XForm xf2 = new XForm();
sweep2.GetXForm(out xf2, toi);
int vertexCount = _shape2.VertexCount;
Vec2[] vertices = new Vec2[Box2DX.Common.Settings.MaxPolygonVertices];
Vec2[] localVertices = _shape2.GetVertices();
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DX.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
localVertices = _shape2.GetCoreVertices();
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DX.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
}
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vector2[] vertices2 = poly2.GetVertices();
Vector2[] normals2 = poly2.Normals;
//Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = CommonMath.MulT(xf2.R, CommonMath.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new ClipVertex[2];
c[0].V = CommonMath.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = CommonMath.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="edge1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] vertices1 = poly1.GetVertices();
Vector2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vector2[] vertices2 = poly2.GetVertices();
//Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = CommonMath.Mul(xf1.R, normals1[edge1]);
Vector2 normal1 = CommonMath.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = CommonMath.Mul(xf1, vertices1[edge1]);
Vector2 v2 = CommonMath.Mul(xf2, vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return(separation);
}
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = CommonMath.Mul(xf2, circle.GetLocalPosition());
Vector2 cLocal = CommonMath.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vector2[] vertices = polygon.GetVertices();
Vector2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vector2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position);
manifold.Points[0].Separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = CommonMath.Normalize(ref e);
//Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vector2.Dot(cLocal - vertices[vertIndex1], e);
Vector2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
}
Vector2 d = cLocal - p;
float dist = CommonMath.Normalize(ref d);
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, d);
Vector2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
