public bool Contains(Vector2d v)
{
Vector2d lv = v - Center;
return((Math.Abs(lv.Dot(AxisX)) <= Extent.x) &&
(Math.Abs(lv.Dot(AxisY)) <= Extent.y));
}
/// <summary>
/// Returns distance to box, or 0 if point is inside box.
/// Ported from WildMagic5 Wm5DistPoint2Box2.cpp
/// </summary>
public double DistanceSquared(Vector2d v)
{
// Work in the box's coordinate system.
v -= this.Center;
// Compute squared distance and closest point on box.
double sqrDistance = 0;
double delta, c, extent;
for (int i = 0; i < 2; ++i)
{
if (i == 0)
{
c = v.Dot(AxisX); extent = Extent.x;
}
else
{
c = v.Dot(AxisY); extent = Extent.y;
}
if (c < -extent)
{
delta = c + extent;
sqrDistance += delta * delta;
}
else if (c > extent)
{
delta = c - extent;
sqrDistance += delta * delta;
}
}
return(sqrDistance);
}
// This is a variant of Fit() where the d1 value is specified.
// Note: has not been tested extensively, particularly the special case
// where one of the arcs beomes a semi-circle...
void Fit(double d1)
{
Vector2d p1 = Point1;
Vector2d p2 = Point2;
Vector2d t1 = Tangent1;
Vector2d t2 = Tangent2;
// fit biarc
Vector2d v = p2 - p1;
double vMagSqr = v.LengthSquared;
// set d1 equal to d2
Vector2d t = t1 + t2;
double tMagSqr = t.LengthSquared;
double vDotT1 = v.Dot(t1);
double vDotT2 = v.Dot(t2);
double t1DotT2 = t1.Dot(t2);
double denominator = (vDotT2 - d1 * (t1DotT2 - 1.0));
if (MathUtil.EpsilonEqual(denominator, 0.0, MathUtil.ZeroTolerancef))
{
// the second arc is a semicircle
FitD1 = d1;
FitD2 = double.PositiveInfinity;
Vector2d joint = p1 + d1 * t1;
joint += (vDotT2 - d1 * t1DotT2) * t2;
// construct arcs
// [TODO] this might not be right for semi-circle...
SetArcFromEdge(0, p1, t1, joint, true);
SetArcFromEdge(1, p2, t2, joint, false);
}
else
{
double d2 = (0.5 * vMagSqr - d1 * vDotT1) / denominator;
double invLen = 1.0 / (d1 + d2);
Vector2d joint = (d1 * d2) * (t1 - t2);
joint += d1 * p2;
joint += d2 * p1;
joint *= invLen;
FitD1 = d1;
FitD2 = d2;
// draw arcs
SetArcFromEdge(0, p1, t1, joint, true);
SetArcFromEdge(1, p2, t2, joint, false);
}
}
void SetArcFromEdge(int i, Vector2d p1, Vector2d t1, Vector2d p2, bool fromP1)
{
Vector2d chord = p2 - p1;
Vector2d n1 = new Vector2d(-t1.y, t1.x);
double chordDotN1 = chord.Dot(n1);
if (MathUtil.EpsilonEqual(chordDotN1, 0, Epsilon))
{
// straight line case
set_arc(i, new Arc(p1, p2));
}
else
{
double radius = chord.LengthSquared / (2.0 * chordDotN1);
Vector2d center = p1 + radius * n1;
Vector2d p1Offset = p1 - center;
Vector2d p2Offset = p2 - center;
var p1Ang1 = Math.Atan2(p1Offset.y, p1Offset.x);
var p2Ang1 = Math.Atan2(p2Offset.y, p2Offset.x);
if (p1Offset.x * t1.y - p1Offset.y * t1.x > 0)
{
set_arc(i, new Arc(center, Math.Abs(radius), p1Ang1, p2Ang1, !fromP1));
}
else
{
set_arc(i, new Arc(center, Math.Abs(radius), p1Ang1, p2Ang1, fromP1));
}
}
}
public Vector2d ClosestPoint(Vector2d v)
{
// Work in the box's coordinate system.
Vector2d diff = v - this.Center;
// Compute squared distance and closest point on box.
double sqrDistance = 0;
double delta;
Vector2d closest = new Vector2d();
for (int i = 0; i < 2; ++i)
{
closest[i] = diff.Dot((i == 0) ? AxisX : AxisY);
double extent = (i == 0) ? Extent.x : Extent.y;
if (closest[i] < -extent)
{
delta = closest[i] + extent;
sqrDistance += delta * delta;
closest[i] = -extent;
}
else if (closest[i] > extent)
{
delta = closest[i] - extent;
sqrDistance += delta * delta;
closest[i] = extent;
}
}
return(closest.x * AxisX + closest.y * AxisY);
}
public static int WhichSide(Triangle2d V, Vector2d P, Vector2d D)
{
// Vertices are projected to the form P+t*D. Return value is +1 if all
// t > 0, -1 if all t < 0, 0 otherwise, in which case the line splits the
// triangle.
int positive = 0, negative = 0, zero = 0;
for (int i = 0; i < 3; ++i)
{
double t = D.Dot(V[i] - P);
if (t > (double)0)
{
++positive;
}
else if (t < (double)0)
{
++negative;
}
else
{
++zero;
}
if (positive > 0 && negative > 0)
{
return(0);
}
}
return(zero == 0 ? (positive > 0 ? 1 : -1) : 0);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Projection of P-C onto plane is Q-C = P-C - Dot(N,P-C)*N.
Vector2d PmC = point - circle.Center;
double lengthPmC = PmC.Length;
if (lengthPmC > MathUtil.Epsilon)
{
CircleClosest = circle.Center + circle.Radius * PmC / lengthPmC;
AllCirclePointsEquidistant = false;
}
else
{
// All circle points are equidistant from P. Return one of them.
CircleClosest = circle.Center + circle.Radius;
AllCirclePointsEquidistant = true;
}
Vector2d diff = point - CircleClosest;
double sqrDistance = diff.Dot(diff);
// Account for numerical round-off error.
if (sqrDistance < 0)
{
sqrDistance = 0;
}
DistanceSquared = sqrDistance;
return(sqrDistance);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector2d diff = line1.Origin - line2.Origin;
double a01 = -line1.Direction.Dot(line2.Direction);
double b0 = diff.Dot(line1.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1.0 - a01 * a01);
double b1, s0, s1, sqrDist;
if (det >= MathUtil.ZeroTolerance)
{
// Lines are not parallel.
b1 = -diff.Dot(line2.Direction);
double invDet = ((double)1) / det;
s0 = (a01 * b1 - b0) * invDet;
s1 = (a01 * b0 - b1) * invDet;
sqrDist = (double)0;
}
else
{
// Lines are parallel, select any closest pair of points.
s0 = -b0;
s1 = (double)0;
sqrDist = b0 * s0 + c;
// Account for numerical round-off errors.
if (sqrDist < (double)0)
{
sqrDist = (double)0;
}
}
Line1Parameter = s0;
Line1Closest = line1.Origin + s0 * line1.Direction;
Line2Parameter = s1;
Line2Closest = line2.Origin + s1 * line2.Direction;
DistanceSquared = sqrDist;
return(sqrDist);
}
// Evaluate the quadratic function Q(X) = (X-K)^T * M * (X-K) - 1.
public double Evaluate(Vector2d point)
{
Vector2d diff = point - Center;
double ratio0 = Axis0.Dot(diff) / Extent[0];
double ratio1 = Axis1.Dot(diff) / Extent[1];
double value = ratio0 * ratio0 + ratio1 * ratio1 - (double)1;
return(value);
}
/// <summary>
/// Compute barycentric coordinates/weights of vPoint inside triangle (V0,V1,V2).
/// If point is inside triangle, coords will pe positive and sum to 1.
/// ie if result is a, then vPoint = a.x*V0 + a.y*V1 + a.z*V2.
/// </summary>
public static Vector3d BarycentricCoords(Vector2d vPoint, Vector2d V0, Vector2d V1, Vector2d V2)
{
Vector2d kV02 = V0 - V2;
Vector2d kV12 = V1 - V2;
Vector2d kPV2 = vPoint - V2;
double fM00 = kV02.Dot(kV02);
double fM01 = kV02.Dot(kV12);
double fM11 = kV12.Dot(kV12);
double fR0 = kV02.Dot(kPV2);
double fR1 = kV12.Dot(kPV2);
double fDet = fM00 * fM11 - fM01 * fM01;
double fInvDet = 1.0 / fDet;
double fBary1 = (fM11 * fR0 - fM01 * fR1) * fInvDet;
double fBary2 = (fM00 * fR1 - fM01 * fR0) * fInvDet;
double fBary3 = 1.0 - fBary1 - fBary2;
return(new Vector3d(fBary1, fBary2, fBary3));
}
protected void UpdateBox(Vector2d LPoint, Vector2d RPoint,
Vector2d BPoint, Vector2d TPoint,
ref Vector2d U, ref Vector2d V, ref double minAreaDiv4)
{
Vector2d RLDiff = RPoint - LPoint;
Vector2d TBDiff = TPoint - BPoint;
double extent0 = ((double)0.5) * (U.Dot(RLDiff));
double extent1 = ((double)0.5) * (V.Dot(TBDiff));
double areaDiv4 = extent0 * extent1;
if (areaDiv4 < minAreaDiv4)
{
minAreaDiv4 = areaDiv4;
mMinBox.AxisX = U;
mMinBox.AxisY = V;
mMinBox.Extent[0] = extent0;
mMinBox.Extent[1] = extent1;
Vector2d LBDiff = LPoint - BPoint;
mMinBox.Center = LPoint + U * extent0 + V * (extent1 - V.Dot(LBDiff));
}
}
public bool Find()
{
if (Result != IntersectionResult.NotComputed)
{
return(Result == IntersectionResult.Intersects);
}
// The potential intersection is initialized to triangle1. The set of
// vertices is refined based on clipping against each edge of triangle0.
Quantity = 3;
Points = new Vector2d[6];
for (int i = 0; i < 3; ++i)
{
Points[i] = triangle1[i];
}
for (int i1 = 2, i0 = 0; i0 < 3; i1 = i0++)
{
// Clip against edge <V0[i1],V0[i0]>.
var N = new Vector2d(
triangle0[i1].y - triangle0[i0].y,
triangle0[i0].x - triangle0[i1].x);
double c = N.Dot(triangle0[i1]);
ClipConvexPolygonAgainstLine(N, c, ref Quantity, ref Points);
if (Quantity == 0)
{
// Triangle completely clipped, no intersection occurs.
Type = IntersectionType.Empty;
}
else if (Quantity == 1)
{
Type = IntersectionType.Point;
}
else if (Quantity == 2)
{
Type = IntersectionType.Segment;
}
else
{
Type = IntersectionType.Polygon;
}
}
Result = (Type != IntersectionType.Empty) ?
IntersectionResult.Intersects : IntersectionResult.NoIntersection;
return(Result == IntersectionResult.Intersects);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Work in the box's coordinate system.
Vector2d diff = point - box.Center;
// Compute squared distance and closest point on box.
double sqrDistance = (double)0;
double delta;
Vector2d closest = Vector2d.Zero;
int i;
for (i = 0; i < 2; ++i)
{
closest[i] = diff.Dot(box.Axis(i));
if (closest[i] < -box.Extent[i])
{
delta = closest[i] + box.Extent[i];
sqrDistance += delta * delta;
closest[i] = -box.Extent[i];
}
else if (closest[i] > box.Extent[i])
{
delta = closest[i] - box.Extent[i];
sqrDistance += delta * delta;
closest[i] = box.Extent[i];
}
}
BoxClosest = box.Center;
for (i = 0; i < 2; ++i)
{
BoxClosest += closest[i] * box.Axis(i);
}
DistanceSquared = sqrDistance;
return(sqrDistance);
}
/// <summary>
/// Test if segments intersect. Returns true for parallel-line overlaps.
/// Returns same result as IntrSegment2Segment2.
/// </summary>
public bool Intersects(ref Segment2d seg2, double dotThresh = double.Epsilon, double intervalThresh = 0)
{
// see IntrLine2Line2 and IntrSegment2Segment2 for details on this code
Vector2d diff = seg2.Center - Center;
double D0DotPerpD1 = Direction.DotPerp(seg2.Direction);
if (Math.Abs(D0DotPerpD1) > dotThresh)
{ // Lines intersect in a single point.
double invD0DotPerpD1 = ((double)1) / D0DotPerpD1;
double diffDotPerpD0 = diff.DotPerp(Direction);
double diffDotPerpD1 = diff.DotPerp(seg2.Direction);
double s = diffDotPerpD1 * invD0DotPerpD1;
double s2 = diffDotPerpD0 * invD0DotPerpD1;
return(Math.Abs(s) <= (Extent + intervalThresh) &&
Math.Abs(s2) <= (seg2.Extent + intervalThresh));
}
// Lines are parallel.
diff.Normalize();
double diffNDotPerpD1 = diff.DotPerp(seg2.Direction);
if (Math.Abs(diffNDotPerpD1) <= dotThresh)
{
// Compute the location of segment1 endpoints relative to segment0.
diff = seg2.Center - Center;
double t1 = Direction.Dot(diff);
double tmin = t1 - seg2.Extent;
double tmax = t1 + seg2.Extent;
var extents = new Interval1d(-Extent, Extent);
if (extents.Overlaps(new Interval1d(tmin, tmax)))
{
return(true);
}
return(false);
}
// lines are parallel but not collinear
return(false);
}
public void Contain(Vector2d v)
{
Vector2d lv = v - Center;
for (int k = 0; k < 2; ++k)
{
double t = lv.Dot(Axis(k));
if (Math.Abs(t) > Extent[k])
{
double min = -Extent[k], max = Extent[k];
if (t < min)
{
min = t;
}
else if (t > max)
{
max = t;
}
Extent[k] = (max - min) * 0.5;
Center = Center + ((max + min) * 0.5) * Axis(k);
}
}
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector2d diff = segment0.Center - segment1.Center;
double a01 = -segment0.Direction.Dot(segment1.Direction);
double b0 = diff.Dot(segment0.Direction);
double b1 = -diff.Dot(segment1.Direction);
double c = diff.LengthSquared;
double det = Math.Abs((double)1 - a01 * a01);
double s0, s1, sqrDist, extDet0, extDet1, tmpS0, tmpS1;
if (det >= MathUtil.ZeroTolerance)
{
// Segments are not parallel.
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
extDet0 = segment0.Extent * det;
extDet1 = segment1.Extent * det;
if (s0 >= -extDet0)
{
if (s0 <= extDet0)
{
if (s1 >= -extDet1)
{
if (s1 <= extDet1) // region 0 (interior)
{
// Minimum at interior points of segments.
double invDet = ((double)1) / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = (double)0;
}
else // region 3 (side)
{
s1 = segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 < -segment0.Extent)
{
s0 = -segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 <= segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
}
}
else // region 7 (side)
{
s1 = -segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 < -segment0.Extent)
{
s0 = -segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 <= segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
}
}
else
{
if (s1 >= -extDet1)
{
if (s1 <= extDet1) // region 1 (side)
{
s0 = segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 < -segment1.Extent)
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 <= segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
else // region 2 (corner)
{
s1 = segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 < -segment0.Extent)
{
s0 = -segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 <= segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 < -segment1.Extent)
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 <= segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
}
}
else // region 8 (corner)
{
s1 = -segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 < -segment0.Extent)
{
s0 = -segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 <= segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 > segment1.Extent)
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 >= -segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
}
}
}
else
{
if (s1 >= -extDet1)
{
if (s1 <= extDet1) // region 5 (side)
{
s0 = -segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 < -segment1.Extent)
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 <= segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
else // region 4 (corner)
{
s1 = segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 > segment0.Extent)
{
s0 = segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 >= -segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = -segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 < -segment1.Extent)
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 <= segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
}
}
else // region 6 (corner)
{
s1 = -segment1.Extent;
tmpS0 = -(a01 * s1 + b0);
if (tmpS0 > segment0.Extent)
{
s0 = segment0.Extent;
sqrDist = s0 * (s0 - ((double)2) * tmpS0) +
s1 * (s1 + ((double)2) * b1) + c;
}
else if (tmpS0 >= -segment0.Extent)
{
s0 = tmpS0;
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
else
{
s0 = -segment0.Extent;
tmpS1 = -(a01 * s0 + b1);
if (tmpS1 < -segment1.Extent)
{
s1 = -segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
else if (tmpS1 <= segment1.Extent)
{
s1 = tmpS1;
sqrDist = -s1 * s1 + s0 * (s0 + ((double)2) * b0) + c;
}
else
{
s1 = segment1.Extent;
sqrDist = s1 * (s1 - ((double)2) * tmpS1) +
s0 * (s0 + ((double)2) * b0) + c;
}
}
}
}
}
else
{
// The segments are parallel. The average b0 term is designed to
// ensure symmetry of the function. That is, dist(seg0,seg1) and
// dist(seg1,seg0) should produce the same number.
double e0pe1 = segment0.Extent + segment1.Extent;
double sign = (a01 > (double)0 ? (double)-1 : (double)1);
double b0Avr = ((double)0.5) * (b0 - sign * b1);
double lambda = -b0Avr;
if (lambda < -e0pe1)
{
lambda = -e0pe1;
}
else if (lambda > e0pe1)
{
lambda = e0pe1;
}
s1 = -sign * lambda * segment1.Extent / e0pe1;
s0 = lambda + sign * s1;
sqrDist = lambda * (lambda + ((double)2) * b0Avr) + c;
}
// Account for numerical round-off errors.
if (sqrDist < (double)0)
{
sqrDist = (double)0;
}
Segment1Parameter = s0;
Segment1Closest = segment0.Center + s0 * segment0.Direction;
Segment2Parameter = s1;
Segment2Closest = segment1.Center + s1 * segment1.Direction;
DistanceSquared = sqrDist;
return(sqrDist);
}
public static void GetInterval(Vector2d origin, Vector2d direction, Triangle2d tri,
Vector3d dist, Vector3i sign, ref Vector2d param)
{
// Project triangle onto line.
Vector3d proj = Vector3d.Zero;
int i;
for (i = 0; i < 3; ++i)
{
Vector2d diff = tri[i] - origin;
proj[i] = direction.Dot(diff);
}
// Compute transverse intersections of triangle edges with line.
double numer, denom;
int i0, i1, i2;
int quantity = 0;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
if (sign[i0] * sign[i1] < 0)
{
if (quantity >= 2)
{
throw new Exception("IntrLine2Triangle2.GetInterval: too many intersections!");
}
numer = dist[i0] * proj[i1] - dist[i1] * proj[i0];
denom = dist[i0] - dist[i1];
param[quantity++] = numer / denom;
}
}
// Check for grazing contact.
if (quantity < 2)
{
for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
{
if (sign[i2] == 0)
{
if (quantity >= 2)
{
throw new Exception("IntrLine2Triangle2.GetInterval: too many intersections!");
}
param[quantity++] = proj[i2];
}
}
}
// Sort.
if (quantity < 1)
{
throw new Exception("IntrLine2Triangle2.GetInterval: need at least one intersection");
}
if (quantity == 2)
{
if (param[0] > param[1])
{
double save = param[0];
param[0] = param[1];
param[1] = save;
}
}
else
{
param[1] = param[0];
}
}
// Vin is input polygon vertices, returns clipped polygon, vertex count of
// clipped polygon is returned in quantity
// **NOTE** returned array may have more elements than quantity!!
public static void ClipConvexPolygonAgainstLine(
Vector2d N, double c, ref int quantity, ref Vector2d[] V)
{
// The input vertices are assumed to be in counterclockwise order. The
// ordering is an invariant of this function.
// Test on which side of line the vertices are.
int positive = 0, negative = 0, pIndex = -1;
double[] test = new double[6];
int i;
for (i = 0; i < quantity; ++i)
{
test[i] = N.Dot(V[i]) - c;
if (test[i] > (double)0)
{
positive++;
if (pIndex < 0)
{
pIndex = i;
}
}
else if (test[i] < (double)0)
{
negative++;
}
}
if (positive > 0)
{
if (negative > 0)
{
// Line transversely intersects polygon.
var CV = new Vector2d[6];
int cQuantity = 0, cur, prv;
double t;
if (pIndex > 0)
{
// First clip vertex on line.
cur = pIndex;
prv = cur - 1;
t = test[cur] / (test[cur] - test[prv]);
CV[cQuantity++] = V[cur] + t * (V[prv] - V[cur]);
// Vertices on positive side of line.
while (cur < quantity && test[cur] > (double)0)
{
CV[cQuantity++] = V[cur++];
}
// Last clip vertex on line.
if (cur < quantity)
{
prv = cur - 1;
}
else
{
cur = 0;
prv = quantity - 1;
}
t = test[cur] / (test[cur] - test[prv]);
CV[cQuantity++] = V[cur] + t * (V[prv] - V[cur]);
}
else // pIndex is 0
{
// Vertices on positive side of line.
cur = 0;
while (cur < quantity && test[cur] > (double)0)
{
CV[cQuantity++] = V[cur++];
}
// Last clip vertex on line.
prv = cur - 1;
t = test[cur] / (test[cur] - test[prv]);
CV[cQuantity++] = V[cur] + t * (V[prv] - V[cur]);
// Skip vertices on negative side.
while (cur < quantity && test[cur] <= (double)0)
{
++cur;
}
// First clip vertex on line.
if (cur < quantity)
{
prv = cur - 1;
t = test[cur] / (test[cur] - test[prv]);
CV[cQuantity++] = V[cur] + t * (V[prv] - V[cur]);
// Vertices on positive side of line.
while (cur < quantity && test[cur] > (double)0)
{
CV[cQuantity++] = V[cur++];
}
}
else
{
// cur = 0
prv = quantity - 1;
t = test[0] / (test[0] - test[prv]);
CV[cQuantity++] = V[0] + t * (V[prv] - V[0]);
}
}
quantity = cQuantity;
Array.Copy(CV, V, cQuantity);
}
// else polygon fully on positive side of line, nothing to do.
}
else
{
// Polygon does not intersect positive side of line, clip all.
quantity = 0;
}
}
// ported from WildMagic5 Wm5ContBox2.cpp::MergeBoxes
public static Box2d Merge(ref Box2d box0, ref Box2d box1)
{
// Construct a box that contains the input boxes.
Box2d box = new Box2d();
// The first guess at the box center. This value will be updated later
// after the input box vertices are projected onto axes determined by an
// average of box axes.
box.Center = 0.5 * (box0.Center + box1.Center);
// The merged box axes are the averages of the input box axes. The
// axes of the second box are negated, if necessary, so they form acute
// angles with the axes of the first box.
if (box0.AxisX.Dot(box1.AxisX) >= 0)
{
box.AxisX = (0.5) * (box0.AxisX + box1.AxisX);
box.AxisX.Normalize();
}
else
{
box.AxisX = (0.5) * (box0.AxisX - box1.AxisX);
box.AxisX.Normalize();
}
box.AxisY = -box.AxisX.Perp;
// Project the input box vertices onto the merged-box axes. Each axis
// D[i] containing the current center C has a minimum projected value
// min[i] and a maximum projected value max[i]. The corresponding end
// points on the axes are C+min[i]*D[i] and C+max[i]*D[i]. The point C
// is not necessarily the midpoint for any of the intervals. The actual
// box center will be adjusted from C to a point C' that is the midpoint
// of each interval,
// C' = C + sum_{i=0}^1 0.5*(min[i]+max[i])*D[i]
// The box extents are
// e[i] = 0.5*(max[i]-min[i])
int i, j;
double dot;
Vector2d diff = new Vector2d();
Vector2d pmin = Vector2d.Zero;
Vector2d pmax = Vector2d.Zero;
Vector2dTuple4 vertex = new Vector2dTuple4();
box0.ComputeVertices(ref vertex);
for (i = 0; i < 4; ++i)
{
diff = vertex[i] - box.Center;
for (j = 0; j < 2; ++j)
{
dot = diff.Dot(box.Axis(j));
if (dot > pmax[j])
{
pmax[j] = dot;
}
else if (dot < pmin[j])
{
pmin[j] = dot;
}
}
}
box1.ComputeVertices(ref vertex);
for (i = 0; i < 4; ++i)
{
diff = vertex[i] - box.Center;
for (j = 0; j < 2; ++j)
{
dot = diff.Dot(box.Axis(j));
if (dot > pmax[j])
{
pmax[j] = dot;
}
else if (dot < pmin[j])
{
pmin[j] = dot;
}
}
}
// [min,max] is the axis-aligned box in the coordinate system of the
// merged box axes. Update the current box center to be the center of
// the new box. Compute the extents based on the new center.
box.Extent[0] = 0.5 * (pmax[0] - pmin[0]);
box.Extent[1] = 0.5 * (pmax[1] - pmin[1]);
box.Center += box.AxisX * (0.5 * (pmax[0] + pmin[0]));
box.Center += box.AxisY * (0.5 * (pmax[1] + pmin[1]));
return(box);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector2d diff = line.Origin - segment.Center;
double a01 = -line.Direction.Dot(segment.Direction);
double b0 = diff.Dot(line.Direction);
double c = diff.LengthSquared;
double det = Math.Abs((double)1 - a01 * a01);
double b1, s0, s1, sqrDist, extDet;
if (det >= MathUtil.ZeroTolerance)
{
// The line and segment are not parallel.
b1 = -diff.Dot(segment.Direction);
s1 = a01 * b0 - b1;
extDet = segment.Extent * det;
if (s1 >= -extDet)
{
if (s1 <= extDet)
{
// Two interior points are closest, one on the line and one
// on the segment.
double invDet = ((double)1) / det;
s0 = (a01 * b1 - b0) * invDet;
s1 *= invDet;
sqrDist = (double)0;
}
else
{
// The endpoint e1 of the segment and an interior point of
// the line are closest.
s1 = segment.Extent;
s0 = -(a01 * s1 + b0);
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
}
else
{
// The endpoint e0 of the segment and an interior point of the
// line are closest.
s1 = -segment.Extent;
s0 = -(a01 * s1 + b0);
sqrDist = -s0 * s0 + s1 * (s1 + ((double)2) * b1) + c;
}
}
else
{
// The line and segment are parallel. Choose the closest pair so that
// one point is at segment origin.
s1 = (double)0;
s0 = -b0;
sqrDist = b0 * s0 + c;
}
LineParameter = s0;
LineClosest = line.Origin + s0 * line.Direction;
SegmentParameter = s1;
SegmentClosest = segment.Center + s1 * segment.Direction;
// Account for numerical round-off errors
if (sqrDist < (double)0)
{
sqrDist = (double)0;
}
DistanceSquared = sqrDist;
return(sqrDist);
}
// u^T*M*v
public double QForm(Vector2d u, Vector2d v)
{
return(u.Dot(this * v));
}
public ContMinBox2(IList <Vector2d> points, double epsilon, QueryNumberType queryType, bool isConvexPolygon)
{
// Get the convex hull of the points.
IList <Vector2d> hullPoints;
int numPoints;
if (isConvexPolygon)
{
hullPoints = points;
numPoints = hullPoints.Count;
}
else
{
ConvexHull2 hull = new ConvexHull2(points, epsilon, queryType);
int hullDim = hull.Dimension;
int hullNumSimplices = hull.NumSimplices;
int[] hullIndices = hull.HullIndices;
if (hullDim == 0)
{
mMinBox.Center = points[0];
mMinBox.AxisX = Vector2d.AxisX;
mMinBox.AxisY = Vector2d.AxisY;
mMinBox.Extent[0] = (double)0;
mMinBox.Extent[1] = (double)0;
return;
}
if (hullDim == 1)
{
throw new NotImplementedException("ContMinBox2: Have not implemented 1d case");
//ConvexHull1 hull1 = hull.GetConvexHull1();
//hullIndices = hull1->GetIndices();
//mMinBox.Center = ((double)0.5) * (points[hullIndices[0]] +
// points[hullIndices[1]]);
//Vector2d diff =
// points[hullIndices[1]] - points[hullIndices[0]];
//mMinBox.Extent[0] = ((double)0.5) * diff.Normalize();
//mMinBox.Extent[1] = (double)0.0;
//mMinBox.Axis[0] = diff;
//mMinBox.Axis[1] = -mMinBox.Axis[0].Perp();
//return;
}
numPoints = hullNumSimplices;
Vector2d[] pointsArray = new Vector2d[numPoints];
for (int i = 0; i < numPoints; ++i)
{
pointsArray[i] = points[hullIndices[i]];
}
hullPoints = pointsArray;
}
// The input points are V[0] through V[N-1] and are assumed to be the
// vertices of a convex polygon that are counterclockwise ordered. The
// input points must not contain three consecutive collinear points.
// Unit-length edge directions of convex polygon. These could be
// precomputed and passed to this routine if the application requires it.
int numPointsM1 = numPoints - 1;
Vector2d[] edges = new Vector2d[numPoints];
bool[] visited = new bool[numPoints];
for (int i = 0; i < numPointsM1; ++i)
{
edges[i] = hullPoints[i + 1] - hullPoints[i];
edges[i].Normalize();
visited[i] = false;
}
edges[numPointsM1] = hullPoints[0] - hullPoints[numPointsM1];
edges[numPointsM1].Normalize();
visited[numPointsM1] = false;
// Find the smallest axis-aligned box containing the points. Keep track
// of the extremum indices, L (left), R (right), B (bottom), and T (top)
// so that the following constraints are met:
// V[L].x <= V[i].x for all i and V[(L+1)%N].x > V[L].x
// V[R].x >= V[i].x for all i and V[(R+1)%N].x < V[R].x
// V[B].y <= V[i].y for all i and V[(B+1)%N].y > V[B].y
// V[T].y >= V[i].y for all i and V[(T+1)%N].y < V[T].y
double xmin = hullPoints[0].x, xmax = xmin;
double ymin = hullPoints[0].y, ymax = ymin;
int LIndex = 0, RIndex = 0, BIndex = 0, TIndex = 0;
for (int i = 1; i < numPoints; ++i)
{
if (hullPoints[i].x <= xmin)
{
xmin = hullPoints[i].x;
LIndex = i;
}
if (hullPoints[i].x >= xmax)
{
xmax = hullPoints[i].x;
RIndex = i;
}
if (hullPoints[i].y <= ymin)
{
ymin = hullPoints[i].y;
BIndex = i;
}
if (hullPoints[i].y >= ymax)
{
ymax = hullPoints[i].y;
TIndex = i;
}
}
// Apply wrap-around tests to ensure the constraints mentioned above are
// satisfied.
if (LIndex == numPointsM1)
{
if (hullPoints[0].x <= xmin)
{
xmin = hullPoints[0].x;
LIndex = 0;
}
}
if (RIndex == numPointsM1)
{
if (hullPoints[0].x >= xmax)
{
xmax = hullPoints[0].x;
RIndex = 0;
}
}
if (BIndex == numPointsM1)
{
if (hullPoints[0].y <= ymin)
{
ymin = hullPoints[0].y;
BIndex = 0;
}
}
if (TIndex == numPointsM1)
{
if (hullPoints[0].y >= ymax)
{
ymax = hullPoints[0].y;
TIndex = 0;
}
}
// The dimensions of the axis-aligned box. The extents store width and
// height for now.
mMinBox.Center.x = ((double)0.5) * (xmin + xmax);
mMinBox.Center.y = ((double)0.5) * (ymin + ymax);
mMinBox.AxisX = Vector2d.AxisX;
mMinBox.AxisY = Vector2d.AxisY;
mMinBox.Extent[0] = ((double)0.5) * (xmax - xmin);
mMinBox.Extent[1] = ((double)0.5) * (ymax - ymin);
double minAreaDiv4 = mMinBox.Extent[0] * mMinBox.Extent[1];
// The rotating calipers algorithm.
Vector2d U = Vector2d.AxisX;
Vector2d V = Vector2d.AxisY;
bool done = false;
while (!done)
{
// Determine the edge that forms the smallest angle with the current
// box edges.
RCFlags flag = RCFlags.F_NONE;
double maxDot = (double)0;
double dot = U.Dot(edges[BIndex]);
if (dot > maxDot)
{
maxDot = dot;
flag = RCFlags.F_BOTTOM;
}
dot = V.Dot(edges[RIndex]);
if (dot > maxDot)
{
maxDot = dot;
flag = RCFlags.F_RIGHT;
}
dot = -U.Dot(edges[TIndex]);
if (dot > maxDot)
{
maxDot = dot;
flag = RCFlags.F_TOP;
}
dot = -V.Dot(edges[LIndex]);
if (dot > maxDot)
{
maxDot = dot;
flag = RCFlags.F_LEFT;
}
switch (flag)
{
case RCFlags.F_BOTTOM:
if (visited[BIndex])
{
done = true;
}
else
{
// Compute box axes with E[B] as an edge.
U = edges[BIndex];
V = -U.Perp;
UpdateBox(hullPoints[LIndex], hullPoints[RIndex],
hullPoints[BIndex], hullPoints[TIndex], ref U, ref V,
ref minAreaDiv4);
// Mark edge visited and rotate the calipers.
visited[BIndex] = true;
if (++BIndex == numPoints)
{
BIndex = 0;
}
}
break;
case RCFlags.F_RIGHT:
if (visited[RIndex])
{
done = true;
}
else
{
// Compute box axes with E[R] as an edge.
V = edges[RIndex];
U = V.Perp;
UpdateBox(hullPoints[LIndex], hullPoints[RIndex],
hullPoints[BIndex], hullPoints[TIndex], ref U, ref V,
ref minAreaDiv4);
// Mark edge visited and rotate the calipers.
visited[RIndex] = true;
if (++RIndex == numPoints)
{
RIndex = 0;
}
}
break;
case RCFlags.F_TOP:
if (visited[TIndex])
{
done = true;
}
else
{
// Compute box axes with E[T] as an edge.
U = -edges[TIndex];
V = -U.Perp;
UpdateBox(hullPoints[LIndex], hullPoints[RIndex],
hullPoints[BIndex], hullPoints[TIndex], ref U, ref V,
ref minAreaDiv4);
// Mark edge visited and rotate the calipers.
visited[TIndex] = true;
if (++TIndex == numPoints)
{
TIndex = 0;
}
}
break;
case RCFlags.F_LEFT:
if (visited[LIndex])
{
done = true;
}
else
{
// Compute box axes with E[L] as an edge.
V = -edges[LIndex];
U = V.Perp;
UpdateBox(hullPoints[LIndex], hullPoints[RIndex],
hullPoints[BIndex], hullPoints[TIndex], ref U, ref V,
ref minAreaDiv4);
// Mark edge visited and rotate the calipers.
visited[LIndex] = true;
if (++LIndex == numPoints)
{
LIndex = 0;
}
}
break;
case RCFlags.F_NONE:
// The polygon is a rectangle.
done = true;
break;
}
}
}
// [TODO]
// - we could get a better fit to the original curve if we use the ability to have separate
// d1 and d2 values. There is a point where d1 > 0 and d2 > 0 where we will get a best-fit.
//
// if d1==0, the first arc degenerates. The second arc degenerates when d2 == 0.
// If either d1 or d2 go negative, then we shouldn't use that result.
// But it's not clear if we can directly compute the positive-d-range...
//
// It does seem like if we solve the d1=d2 case, then we use [0,2*d1] as the d1 range,
// then maybe we are safe. And we can always discard solutions where it is negative...
// solve biarc fit where the free parameter is automatically set so that
// d1=d2, which is basically the 'middle' case
void Fit()
{
// get inputs
Vector2d p1 = Point1;
Vector2d p2 = Point2;
Vector2d t1 = Tangent1;
Vector2d t2 = Tangent2;
// fit biarc
Vector2d v = p2 - p1;
double vMagSqr = v.LengthSquared;
// set d1 equal to d2
Vector2d t = t1 + t2;
double tMagSqr = t.LengthSquared;
// original code used 0.0001 here...
bool equalTangents = MathUtil.EpsilonEqual(tMagSqr, 4.0, Epsilon);
//var equalTangents = IsEqualEps(tMagSqr, 4.0);
double vDotT1 = v.Dot(t1);
bool perpT1 = MathUtil.EpsilonEqual(vDotT1, 0.0, Epsilon);
if (equalTangents && perpT1)
{
// we have two semicircles
//Vector2d joint = p1 + 0.5 * v;
// d1 = d2 = infinity here...
FitD1 = FitD2 = double.PositiveInfinity;
// draw arcs
double angle = Math.Atan2(v.y, v.x);
Vector2d center1 = p1 + 0.25 * v;
Vector2d center2 = p1 + 0.75 * v;
double radius = Math.Sqrt(vMagSqr) * 0.25;
double cross = v.x * t1.y - v.y * t1.x;
arc1 = new Arc(center1, radius, angle, angle + Math.PI, (cross < 0));
arc1 = new Arc(center2, radius, angle, angle + Math.PI, (cross > 0));
}
else
{
double vDotT = v.Dot(t);
// [RMS] this was unused in original code...
//bool perpT1 = MathUtil.EpsilonEqual(vDotT1, 0, epsilon);
double d1 = 0;
if (equalTangents)
{
d1 = vMagSqr / (4 * vDotT1);
}
else
{
double denominator = 2.0 - 2.0 * t1.Dot(t2);
double discriminant = vDotT * vDotT + denominator * vMagSqr;
d1 = (Math.Sqrt(discriminant) - vDotT) / denominator;
}
FitD1 = FitD2 = d1;
Vector2d joint = p1 + p2 + d1 * (t1 - t2);
joint *= 0.5;
// construct arcs
SetArcFromEdge(0, p1, t1, joint, true);
SetArcFromEdge(1, p2, t2, joint, false);
}
}
