//[UnitTest]
static bool Run()
{
Fixture[] fixes = GetFixtures();
for (int i = 0; i < fixes.Length; ++i)
{
Fixture fix = fixes[i];
double tAB, tPQ;
bool h1 = SegmentCollision.HitTest(fix.a, fix.b, fix.p, fix.q, out tAB, out tPQ);
bool h2 = SegmentCollision.HitTest(fix.a, fix.b, fix.p, fix.q);
if (h1 != h2)
{
Console.Error.WriteLine(String.Format("Test fixture [{0}] did not match across algorithms!", fix.caseName));
Console.Error.WriteLine("h1={0}, h2={1}, tAB={2}, tPQ={3}",
h1, h2, tAB, tPQ);
return(false);
}
else if (h1 != fix.expected)
{
Console.Error.WriteLine(String.Format("Test fixture [{0}] did not match expected result.", fix.caseName));
return(false);
}
else
{
Console.WriteLine(String.Format("Test fixture [{0}] passed.", fix.caseName));
}
}
return(true);
}
public static void AnalyzeClosedness(Stroke stroke, double tolerance,
out bool closed, out Point[] vertices) // out double tAB, out double tPQ)
{
closed = false;
vertices = null;
// Formulate closure gap-tolerance, based on stroke length (but clipped to <= 500isu).
double len = StrokeGeometry.IntegrateLength(stroke);
double segtol = Math.Max(50, Math.Min(tolerance, len / 20.0));
double gaptol = Math.Max(150, Math.Min(tolerance, len / 7.0));
dbg.WriteLine(String.Format("length: {0}", len));
dbg.WriteLine(String.Format("segtol: {0}", segtol));
dbg.WriteLine(String.Format("gaptol: {0}", gaptol));
// Break the stroke down into indices.
int[] indices;
vertices = SegmentizeStroke(stroke, segtol, out indices);
int nv = vertices.Length;
// Do the head/tail segments intersect? Are they close?
if (nv >= 4)
{
Point headA = (Point)vertices[0];
Point headB = (Point)vertices[1];
Point tailP = (Point)vertices[nv - 2];
Point tailQ = (Point)vertices[nv - 1];
double tAB, tPQ;
SegmentCollision.HitTest(headA, headB, tailP, tailQ, out tAB, out tPQ);
Point virtualIntersectH = Geometry.Interpolate(headA, headB, tAB);
Point virtualIntersectT = Geometry.Interpolate(tailP, tailQ, tPQ);
Point virtualIntersect = Geometry.Interpolate(virtualIntersectH, virtualIntersectT, 0.5);
double dh2i = Geometry.DistanceBetween(headA, virtualIntersect);
double dt2i = Geometry.DistanceBetween(tailQ, virtualIntersect);
#if DEBUG
dbg.WriteLine(String.Format("numV: {0}", nv));
dbg.WriteLine(String.Format("tAB: {0}", tAB));
dbg.WriteLine(String.Format("tPQ: {0}", tPQ));
dbg.WriteLine(String.Format("isct: {0}", virtualIntersect));
dbg.WriteLine(String.Format("dh2i: {0}", dh2i));
dbg.WriteLine(String.Format("dt2i: {0}", dt2i));
#endif
if (dh2i < gaptol && dt2i < gaptol)
{
closed = true;
// Adjust the head point to the actual intersection.
vertices[0] = virtualIntersect;
dbg.WriteLine("Closed! Why? dh/t2i < gaptol");
}
else if ((-0.3 < tAB && tAB < 0.3) && (0.7 < tPQ && tPQ < 1.3))
{
closed = true;
// Adjust the head point to the actual intersection.
vertices[0] = virtualIntersect;
dbg.WriteLine("Closed! Why? |t*| < 0.3");
}
else
{
// Last chance: measure nearest distance from head to tail segment.
int closeI = StrokeGeometry.FindClosestPointTo(
stroke, headA, indices[nv - 2], indices[nv - 1]);
Point close = stroke.GetPoint(closeI);
double d = Geometry.DistanceBetween(headA, close);
if (d < gaptol)
{
closed = true; // Keep the head point; discard the tail point below.
dbg.WriteLine("Closed! Why? Last chance head/tail distance < gaptol");
}
}
// Remove the last point as redundant if it's closed.
if (closed)
{
Point[] verticesX = new Point[nv - 1];
Array.Copy(vertices, verticesX, nv - 1);
vertices = verticesX;
}
}
}
// For now, just return whether collision has occurred.
private bool FindIntersection(RigidBodyBase body1, RigidBodyBase body2,
out Point contactPoint, out PointF normal)
{
// Initialize out variables.
contactPoint = new Point(0, 0);
normal = new PointF(0, 0);
if (!body1.BoundingBox.IntersectsWith(body2.BoundingBox))
{
return(false);
}
if (ConnectedByJoint(body1, body2))
{
return(false);
}
Point[] vertices1, vertices2;
if (body1 is PolygonalBody)
{
vertices1 = (Point[])((PolygonalBody)body1).Vertices.Clone();
body1.displacement.TransformPoints(vertices1);
}
else
{
// Approximate vertices for ellipse.
vertices1 = ((EllipticalBody)body1).GetPoints();
}
if (body2 is PolygonalBody)
{
vertices2 = (Point[])((PolygonalBody)body2).Vertices.Clone();
body2.displacement.TransformPoints(vertices2);
}
else
{
// Approximate vertices for ellipse.
vertices2 = ((EllipticalBody)body2).GetPoints();
}
// Loop through each segment and look for intersections.
ArrayList intersections = new ArrayList();
for (int i = 0; i < vertices1.Length; i++)
{
for (int j = 0; j < vertices2.Length; j++)
{
double tAB, tPQ;
Point v1 = vertices1[i];
Point v1Next = vertices1[(i + 1) % vertices1.Length];
Point v2 = vertices2[j];
Point v2Next = vertices2[(j + 1) % vertices2.Length];
bool hit = SegmentCollision.HitTest(v1, v1Next, v2, v2Next,
out tAB, out tPQ);
if (hit)
{
// Find intersections from here.
Point intersection = new Point((int)(v1.X + (v1Next.X - v1.X) * tAB),
(int)(v1.Y + (v1Next.Y - v1.Y) * tAB));
intersections.Add(intersection);
}
}
}
// If no intersections, then no collisions.
if (intersections.Count == 0)
{
return(false);
}
// Get average intersection.
int sumX = 0;
int sumY = 0;
foreach (Point intersection in intersections)
{
sumX += intersection.X;
sumY += intersection.Y;
}
contactPoint = new Point(sumX / intersections.Count,
sumY / intersections.Count);
// Find normal by constructing orthogonal vector from first/last intersections.
// Note: this can be improved by doing a linear fit through the intersections.
Point i0 = (Point)intersections[0];
Point i1 = (Point)intersections[intersections.Count - 1];
// Create a normal vector.
Vector normalVec = new Vector(i1.Y - i0.Y, i0.X - i1.X);
// Compare to vector between contact point and body1.
Vector collToBody1 =
new Vector(contactPoint.X - body1.CG.X, contactPoint.Y - body1.CG.Y);
// If in the opposite direction, reverse normal.
if (collToBody1.Dot(normalVec) < 0)
{
normalVec = normalVec * -1.0;
}
// Normalize.
double normalLength = normalVec.Length;
if (normalLength > 0)
{
normal = new PointF((float)(normalVec.DX / normalLength),
(float)(normalVec.DY / normalLength));
}
else
{
normal = new PointF(0, 0);
}
return(true);
}
private void ReconstructIdealizedPolygon()
{
int n = vertices.Length;
// An extra spot for a temporary "tailpoint".
Point[] idealvertsx = new Point[n + 1];
// First, find longest segment (to use as starting point).
int longesti = -1;
double longest = -1;
for (int i = 0; i < n; ++i)
{
if (this.idealsidelengths[i] > longest)
{
longest = this.idealsidelengths[i]; longesti = i;
}
}
int longestj = longesti + 1;
if (longestj >= n)
{
longestj = 0; //wrap
}
// Start at vertex ahead of longest segment, and go from there.
idealvertsx[0] = vertices[longesti];
double theta = Math.Atan2(vertices[longestj].Y - vertices[longesti].Y,
vertices[longestj].X - vertices[longesti].X);
for (int i = 0; i < n; ++i)
{
int ii = longesti + i;
if (ii >= n)
{
ii -= n; //wrap
}
idealvertsx[i + 1] = Geometry.OffsetPolar(
idealvertsx[i], theta, idealsidelengths[ii]);
int jj = ii + 1;
if (jj >= n)
{
jj = 0; // wrap
}
theta += Geometry.Deg2Rad(idealanglescomplement[jj]);
}
// Replace headpoint and tailpoint with virtual intersection.
double tAB, tPQ;
Point headA = idealvertsx[0], headB = idealvertsx[1],
tailP = idealvertsx[n - 1], tailQ = idealvertsx[n - 0];
SegmentCollision.HitTest(headA, headB, tailP, tailQ, out tAB, out tPQ);
Point virtualIntersectH = Geometry.Interpolate(headA, headB, tAB);
Point virtualIntersectT = Geometry.Interpolate(tailP, tailQ, tPQ);
Point virtualIntersect = Geometry.Interpolate(virtualIntersectH, virtualIntersectT, 0.5);
this.idealverts = new Point[n];
Array.Copy(idealvertsx, idealverts, n);
idealverts[0] = virtualIntersect;
// Recenter on original center of gravity (CG).
Point oldcg = Geometry.EstimatePolygonCentroid(vertices);
Point newcg = Geometry.EstimatePolygonCentroid(idealverts);
using (Matrix m = new Matrix())
{
m.Translate(oldcg.X - newcg.X, oldcg.Y - newcg.Y);
m.TransformPoints(idealverts);
}
}
