/// <summary> Construct a contourwalker.
///
/// </summary>
/// <param name="verts">The vertices of the polygon which's contour is being followed
/// </param>
/// <param name="firstVert">The index of the vertex where the contour's subsection which we
/// walk on starts
/// </param>
/// <param name="lastVert">The index of the vertex where the contour's subsection which we
/// walk on ends
/// </param>
/// <param name="isBackwards">True iff we're walking backwards over the contour
/// </param>
public ContourWalker(PenetrationSweep sweep, Vector2f[] verts, int firstVert, int lastVert, bool isBackwards)
{
InitBlock(sweep);
if (firstVert < 0 || lastVert < 0)
throw new System.ArgumentException("Vertex numbers cannot be negative.");
if (firstVert > verts.Length || lastVert > verts.Length)
throw new System.ArgumentException("The given vertex array doesn't include the first or the last vertex.");
this.isBackwards = isBackwards;
this.verts = verts;
this.firstVert = firstVert;
this.lastVert = lastVert;
this.currentVert = isBackwards?lastVert:firstVert;
this.distance = verts[currentVert].Dot(Parent.sweepDir);
this.penetration = verts[currentVert].Dot(Parent.normal);
CalculateNextValues();
}
private void InitBlock(PenetrationSweep parent)
{
this.parent = parent;
}
/// <summary> Given two intersecting polygons, the intersection points and a collision
/// normal, get the maximum penetration Distance along the normal.
///
/// </summary>
/// <param name="in">The ingoing intersection
/// </param>
/// <param name="out">The outgoing intersection
/// </param>
/// <param name="normal">The collision normal
/// </param>
/// <param name="vertsA">The vertices of polygon A
/// </param>
/// <param name="vertsB">The vertices of polygon B
/// </param>
/// <returns> the maximum penetration depth along the given normal
/// </returns>
public static float GetPenetrationDepth(Intersection ingoing, Intersection outgoing, Vector2f normal, Vector2f[] vertsA, Vector2f[] vertsB)
{
Vector2f sweepdir = new Vector2f(outgoing.position);
sweepdir.Sub(ingoing.position);
PenetrationSweep ps = new PenetrationSweep(normal, sweepdir, ingoing.position, outgoing.position);
//TODO: most penetrations are very simple, similar to:
// \ + |
// \ / \ |
// +-----------x---x-----+
// / \
// these should be handled separately
ContourWalker walkerA = new Silver.Weight.Raw.Collide.PenetrationSweep.ContourWalker(ps, vertsA, ingoing.edgeA, outgoing.edgeA, false);
ContourWalker walkerB = new Silver.Weight.Raw.Collide.PenetrationSweep.ContourWalker(ps, vertsB, (outgoing.edgeB + 1) % vertsB.Length, (ingoing.edgeB + 1) % vertsB.Length, true);
float penetration = 0;
float lowerBound = ingoing.position.Dot(normal);
float upperBound = lowerBound;
while (walkerA.HasNext() || walkerB.HasNext())
{
// if walker a has more and the Next vertex comes before B's
// or if walker a has more but walker b hasn't, go and take a Step
if (walkerA.HasNext() && (walkerA.NextDistance < walkerB.NextDistance || !walkerB.HasNext()))
{
walkerA.Next();
if (walkerA.Distance < ps.startDist || walkerA.Distance > ps.endDist)
continue; // we don't care for vertices outside of the intersecting borders
upperBound = walkerA.GetPenetration();
lowerBound = walkerB.GetPenetration(walkerA.Distance);
}
else
{
walkerB.Next();
if (walkerB.Distance < ps.startDist || walkerB.Distance > ps.endDist)
continue;
upperBound = walkerA.GetPenetration(walkerB.Distance);
lowerBound = walkerB.GetPenetration();
}
penetration = System.Math.Max(penetration, upperBound - lowerBound);
}
return penetration;
}
