/// <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; }