public static void TriangleLineRelations(
Vector2d origin, Vector2d direction,
Triangle2d tri, ref Vector3d dist, ref Vector3i sign,
ref int positive, ref int negative, ref int zero)
{
positive = 0;
negative = 0;
zero = 0;
for (int i = 0; i < 3; ++i)
{
Vector2d diff = tri[i] - origin;
dist[i] = diff.DotPerp(direction);
if (dist[i] > MathUtil.ZeroTolerance)
{
sign[i] = 1;
++positive;
}
else if (dist[i] < -MathUtil.ZeroTolerance)
{
sign[i] = -1;
++negative;
}
else
{
dist[i] = 0.0;
sign[i] = 0;
++zero;
}
}
}
public static IntersectionType Classify(Vector2d P0, Vector2d D0, Vector2d P1, Vector2d D1,
double dotThreshold, ref Vector2d s)
{
// Ensure dotThreshold is nonnegative.
dotThreshold = Math.Max(dotThreshold, (double)0);
// The intersection of two lines is a solution to P0+s0*D0 = P1+s1*D1.
// Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q. If D0.Dot(Perp(D1)) = 0,
// the lines are parallel. Additionally, if Q.Dot(Perp(D1)) = 0, the
// lines are the same. If D0.Dot(Perp(D1)) is not zero, then
// s0 = Q.Dot(Perp(D1))/D0.Dot(Perp(D1))
// produces the point of intersection. Also,
// s1 = Q.Dot(Perp(D0))/D0.Dot(Perp(D1))
Vector2d diff = P1 - P0;
double D0DotPerpD1 = D0.DotPerp(D1);
if (Math.Abs(D0DotPerpD1) > dotThreshold)
{
// Lines intersect in a single point.
double invD0DotPerpD1 = ((double)1) / D0DotPerpD1;
double diffDotPerpD0 = diff.DotPerp(D0);
double diffDotPerpD1 = diff.DotPerp(D1);
s[0] = diffDotPerpD1 * invD0DotPerpD1;
s[1] = diffDotPerpD0 * invD0DotPerpD1;
return(IntersectionType.Point);
}
// Lines are parallel.
diff.Normalize();
double diffNDotPerpD1 = diff.DotPerp(D1);
if (Math.Abs(diffNDotPerpD1) <= dotThreshold)
{
// Lines are colinear.
return(IntersectionType.Line);
}
// Lines are parallel, but distinct.
return(IntersectionType.Empty);
}
/// <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);
}
/// <summary>
/// Calculate intersection point between this line and another one.
/// Returns Vector2d.MaxValue if lines are parallel.
/// </summary>
/// <returns></returns>
public Vector2d IntersectionPoint(ref Line2d other, double dotThresh = MathUtil.ZeroTolerance)
{
// see IntrLine2Line2 for explanation of algorithm
Vector2d diff = other.Origin - Origin;
double D0DotPerpD1 = Direction.DotPerp(other.Direction);
if (Math.Abs(D0DotPerpD1) > dotThresh) // Lines intersect in a single point.
{
double invD0DotPerpD1 = ((double)1) / D0DotPerpD1;
double diffDotPerpD1 = diff.DotPerp(other.Direction);
double s = diffDotPerpD1 * invD0DotPerpD1;
return(Origin + s * Direction);
}
// Lines are parallel.
return(Vector2d.MaxValue);
}
public double GetCurvature(double t)
{
Vector2d der1 = GetFirstDerivative(t);
Vector2d der2 = GetSecondDerivative(t);
double speedSqr = der1.LengthSquared;
if (speedSqr >= MathUtil.ZeroTolerance)
{
double numer = der1.DotPerp(der2);
double denom = Math.Pow(speedSqr, (double)1.5);
return(numer / denom);
}
else
{
// Curvature is indeterminate, just return 0.
return((double)0);
}
}
bool GetCoplanarIntersection(ref Plane3d plane, ref Triangle3d tri0, ref Triangle3d tri1)
{
// Project triangles onto coordinate plane most aligned with plane
// normal.
int maxNormal = 0;
double fmax = Math.Abs(plane.Normal.x);
double absMax = Math.Abs(plane.Normal.y);
if (absMax > fmax)
{
maxNormal = 1;
fmax = absMax;
}
absMax = Math.Abs(plane.Normal.z);
if (absMax > fmax)
{
maxNormal = 2;
}
Triangle2d projTri0 = new Triangle2d(), projTri1 = new Triangle2d();
int i;
if (maxNormal == 0)
{
// Project onto yz-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].yz;
projTri1[i] = tri1[i].yz;
}
}
else if (maxNormal == 1)
{
// Project onto xz-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].xz;
projTri1[i] = tri1[i].xz;
}
}
else
{
// Project onto xy-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].xy;
projTri1[i] = tri1[i].xy;
}
}
// 2D triangle intersection routines require counterclockwise ordering.
Vector2d save;
Vector2d edge0 = projTri0[1] - projTri0[0];
Vector2d edge1 = projTri0[2] - projTri0[0];
if (edge0.DotPerp(edge1) < (double)0)
{
// Triangle is clockwise, reorder it.
save = projTri0[1];
projTri0[1] = projTri0[2];
projTri0[2] = save;
}
edge0 = projTri1[1] - projTri1[0];
edge1 = projTri1[2] - projTri1[0];
if (edge0.DotPerp(edge1) < (double)0)
{
// Triangle is clockwise, reorder it.
save = projTri1[1];
projTri1[1] = projTri1[2];
projTri1[2] = save;
}
IntrTriangle2Triangle2 intr = new IntrTriangle2Triangle2(projTri0, projTri1);
if (!intr.Find())
{
return(false);
}
PolygonPoints = new Vector3d[intr.Quantity];
// Map 2D intersections back to the 3D triangle space.
Quantity = intr.Quantity;
if (maxNormal == 0)
{
double invNX = ((double)1) / plane.Normal.x;
for (i = 0; i < Quantity; i++)
{
double y = intr.Points[i].x;
double z = intr.Points[i].y;
double x = invNX * (plane.Constant - plane.Normal.y * y - plane.Normal.z * z);
PolygonPoints[i] = new Vector3d(x, y, z);
}
}
else if (maxNormal == 1)
{
double invNY = ((double)1) / plane.Normal.y;
for (i = 0; i < Quantity; i++)
{
double x = intr.Points[i].x;
double z = intr.Points[i].y;
double y = invNY * (plane.Constant - plane.Normal.x * x - plane.Normal.z * z);
PolygonPoints[i] = new Vector3d(x, y, z);
}
}
else
{
double invNZ = ((double)1) / plane.Normal.z;
for (i = 0; i < Quantity; i++)
{
double x = intr.Points[i].x;
double y = intr.Points[i].y;
double z = invNZ * (plane.Constant - plane.Normal.x * x - plane.Normal.y * y);
PolygonPoints[i] = new Vector3d(x, y, z);
}
}
Result = IntersectionResult.Intersects;
Type = IntersectionType.Polygon;
return(true);
}