/// <summary> Set a contact for an intersection where the colliding line's start- or endpoint
/// is contained in the colliding polygon.
///
/// TODO: The current implementation doesn't work properly: because lines are very
/// thin, they can slide into a polygon sideways which gives a very deep penetration
/// |
/// |->
/// | +-----+
/// |-> | |
/// | | |
/// | |
/// +-----+
///
/// A possible solution would be to use the velocity of the line relative to the
/// polygon to construct a collision normal and penetration depth.
/// Another possibility is to use the line's normals (both directions) and calculate
/// proper intersection distances for them.
/// If one has multiple normals/penetration depths to choose from, the one with the
/// minimum penetration depth will probably be the best bet.
///
/// </summary>
/// <param name="contact">The contact to set
/// </param>
/// <param name="intersection">The intersection where the line enters or exits the polygon
/// </param>
/// <param name="vertsA">The line's vertices
/// </param>
/// <param name="vertsB">The polygon's vertices
/// </param>
public virtual void SetLineEndContact(Contact contact, Intersection intersection, Vector2f[] vertsA, Vector2f[] vertsB)
{
Vector2f separation = new Vector2f(intersection.position);
if (intersection.isIngoing)
separation.Sub(vertsA[1]);
else
separation.Sub(vertsA[0]);
float depthA = 0; //separation.Length();
contact.Separation = - depthA;
contact.Normal = MathUtil.GetNormal(vertsB[(intersection.edgeB + 1) % vertsB.Length], vertsB[intersection.edgeB]);
contact.Position = intersection.position;
contact.Feature = new FeaturePair(0, 0, intersection.edgeA, intersection.edgeB);
}
/// <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;
}
/// <summary> Given a list of intersections, calculate the collision information and
/// set the contacts with that information.
///
/// </summary>
/// <param name="contacts">The array of contacts to fill
/// </param>
/// <param name="vertsA">The vertices of polygon A
/// </param>
/// <param name="vertsB">The vertices of polygon B
/// </param>
/// <param name="intersections">The array of intersection as returned by
/// {@link IntersectionGatherer#getIntersections()}
/// </param>
/// <returns> The number of contacts that have been set in the contact array
/// </returns>
public virtual int PopulateContacts(Contact[] contacts, Vector2f[] vertsA, Vector2f[] vertsB, Intersection[] intersections)
{
if (intersections.Length == 0)
return 0;
int noContacts = 0;
// is the first intersection outgoing?
if (!intersections[0].isIngoing)
{
SetLineEndContact(contacts[noContacts], intersections[intersections.Length - 1], vertsA, vertsB);
if (contacts[noContacts].Separation < - 10)
System.Console.Out.WriteLine("first " + contacts[noContacts].Separation);
noContacts++;
}
int i = noContacts;
while (i < intersections.Length - 1)
{
if (noContacts > contacts.Length - 2)
return noContacts;
// check if we have an intersection pair
if (!intersections[i].isIngoing || intersections[i + 1].isIngoing)
{
SetContact(contacts[noContacts], intersections[i], vertsA, vertsB);
i++;
noContacts++;
continue;
}
SetContactPair(contacts[noContacts], contacts[noContacts + 1], intersections[i], intersections[i + 1], vertsA, vertsB);
if (contacts[noContacts].Separation < - 10)
System.Console.Out.WriteLine("m " + contacts[noContacts].Separation);
noContacts += 2;
i += 2;
}
// is there still an ingoing intersection left?
if (i < intersections.Length && intersections[intersections.Length - 1].isIngoing && noContacts < contacts.Length)
{
SetLineEndContact(contacts[noContacts], intersections[intersections.Length - 1], vertsA, vertsB);
if (contacts[noContacts].Separation < - 10)
System.Console.Out.WriteLine(" last " + contacts[noContacts].Separation);
noContacts++;
}
return noContacts;
}
