public void EllipseAngleContainmentTest()
{
var el = new PrimitiveEllipse(Point.Origin, 1.0, 90.0, 360.0, Vector.ZAxis);
Assert.True(el.IsAngleContained(90.0));
Assert.True(el.IsAngleContained(180.0));
Assert.True(el.IsAngleContained(270.0));
Assert.True(el.IsAngleContained(360.0));
Assert.False(el.IsAngleContained(45.0));
}
public static IEnumerable <Point> IntersectionPoints(this PrimitiveEllipse ellipse, PrimitivePoint point, bool withinBounds = true)
{
var fromUnit = ellipse.FromUnitCircle;
var toUnit = fromUnit.Inverse();
var pointOnUnit = (Vector)toUnit.Transform(point.Location);
if (MathHelper.CloseTo(pointOnUnit.LengthSquared, 1.0))
{
if (!withinBounds)
{
return new Point[] { point.Location }
}
; // always a match
else
{
var pointAngle = pointOnUnit.ToAngle();
if (ellipse.IsAngleContained(pointAngle))
{
return new Point[] { point.Location }
}
;
else
{
return(new Point[0]);
}
}
}
else
{
// wrong distance
return(new Point[0]);
}
}
private static bool IsPointOnPrimitive(this PrimitiveEllipse el, Point point)
{
var unitPoint = el.FromUnitCircle.Inverse().Transform(point);
return(MathHelper.CloseTo(0.0, unitPoint.Z) && // on the XY plane
MathHelper.CloseTo(1.0, ((Vector)unitPoint).LengthSquared) && // on the unit circle
el.IsAngleContained(Math.Atan2(unitPoint.Y, unitPoint.X) * MathHelper.RadiansToDegrees)); // within angle bounds
}
public static IEnumerable <Point> IntersectionPoints(this PrimitiveEllipse first, PrimitiveEllipse second, bool withinBounds = true)
{
var empty = Enumerable.Empty <Point>();
IEnumerable <Point> results;
// planes either intersect in a line or are parallel
var lineVector = first.Normal.Cross(second.Normal);
if (lineVector.IsZeroVector)
{
// parallel or the same plane
if ((first.Center == second.Center) ||
Math.Abs((second.Center - first.Center).Dot(first.Normal)) < MathHelper.Epsilon)
{
// if they share a point or second.Center is on the first plane, they are the same plane
// project second back to a unit circle and find intersection points
var fromUnit = first.FromUnitCircle;
var toUnit = fromUnit.Inverse();
// transform second ellipse to be on the unit circle's plane
var secondCenter = toUnit.Transform(second.Center);
var secondMajorEnd = toUnit.Transform(second.Center + second.MajorAxis);
var secondMinorEnd = toUnit.Transform(second.Center + (second.Normal.Cross(second.MajorAxis).Normalize() * second.MajorAxis.Length * second.MinorAxisRatio));
RoundedDouble a = (secondMajorEnd - secondCenter).Length;
RoundedDouble b = (secondMinorEnd - secondCenter).Length;
if (a == b)
{
// if second ellipse is a circle we can absolutely solve for the intersection points
// rotate to place the center of the second circle on the x-axis
var angle = ((Vector)secondCenter).ToAngle();
var rotation = Matrix4.RotateAboutZ(angle);
var returnTransform = fromUnit * Matrix4.RotateAboutZ(-angle);
var newSecondCenter = rotation.Transform(secondCenter);
var secondRadius = a;
if (Math.Abs(newSecondCenter.X) > secondRadius + 1.0)
{
// no points of intersection
results = empty;
}
else
{
// 2 points of intersection
var x = (secondRadius * secondRadius - newSecondCenter.X * newSecondCenter.X - 1.0)
/ (-2.0 * newSecondCenter.X);
var y = Math.Sqrt(1.0 - x * x);
results = new[]
{
new Point(x, y, 0),
new Point(x, -y, 0)
};
}
results = results.Distinct().Select(p => returnTransform.Transform(p));
}
else
{
// rotate about the origin to make the major axis align with the x-axis
var angle = (secondMajorEnd - secondCenter).ToAngle();
var rotation = Matrix4.RotateAboutZ(angle);
var finalCenter = rotation.Transform(secondCenter);
fromUnit = fromUnit * Matrix4.RotateAboutZ(-angle);
toUnit = fromUnit.Inverse();
if (a < b)
{
// rotate to ensure a > b
fromUnit = fromUnit * Matrix4.RotateAboutZ(90);
toUnit = fromUnit.Inverse();
finalCenter = Matrix4.RotateAboutZ(-90).Transform(finalCenter);
// and swap a and b
var temp = a;
a = b;
b = temp;
}
RoundedDouble h = finalCenter.X;
RoundedDouble k = finalCenter.Y;
var h2 = h * h;
var k2 = k * k;
var a2 = a * a;
var b2 = b * b;
var a4 = a2 * a2;
var b4 = b2 * b2;
// RoundedDouble is used to ensure all operations are rounded to a certain precision
if (Math.Abs(h) < MathHelper.Epsilon)
{
// ellipse x = 0; wide
// find x values
var A = a2 - a2 * b2 + a2 * k2 - ((2 * a4 * k2) / (a2 - b2));
var B = (2 * a2 * k * Math.Sqrt(b2 * (-a2 + a4 + b2 - a2 * b2 + a2 * k2))) / (a2 - b2);
var C = (RoundedDouble)Math.Sqrt(a2 - b2);
var x1 = -Math.Sqrt(A + B) / C;
var x2 = (RoundedDouble)(-x1);
var x3 = -Math.Sqrt(A - B) / C;
var x4 = (RoundedDouble)(-x3);
// find y values
var D = a2 * k;
var E = Math.Sqrt(-a2 * b2 + a4 * b2 + b4 - a2 * b4 + a2 * b2 * k2);
var F = a2 - b2;
var y1 = (D - E) / F;
var y2 = (D + E) / F;
results = new[] {
new Point(x1, y1, 0),
new Point(x2, y1, 0),
new Point(x3, y2, 0),
new Point(x4, y2, 0)
};
}
else if (Math.Abs(k) < MathHelper.Epsilon)
{
// ellipse y = 0; wide
// find x values
var A = -b2 * h;
var B = Math.Sqrt(a4 - a2 * b2 - a4 * b2 + a2 * b4 + a2 * b2 * h2);
var C = a2 - b2;
var x1 = (A - B) / C;
var x2 = (A + B) / C;
// find y values
var D = -b2 + a2 * b2 - b2 * h2 - ((2 * b4 * h2) / (a2 - b2));
var E = (2 * b2 * h * Math.Sqrt(a2 * (a2 - b2 - a2 * b2 + b4 + b2 * h2))) / (a2 - b2);
var F = (RoundedDouble)Math.Sqrt(a2 - b2);
var y1 = -Math.Sqrt(D - E) / F;
var y2 = (RoundedDouble)(-y1);
var y3 = -Math.Sqrt(D + E) / F;
var y4 = (RoundedDouble)(-y3);
results = new[] {
new Point(x1, y1, 0),
new Point(x1, y2, 0),
new Point(x2, y3, 0),
new Point(x2, y4, 0)
};
}
else
{
// brute-force approximate intersections
results = BruteForceEllipseWithUnitCircle(finalCenter, a, b);
}
results = results
.Where(p => !(double.IsNaN(p.X) || double.IsNaN(p.Y) || double.IsNaN(p.Z)))
.Where(p => !(double.IsInfinity(p.X) || double.IsInfinity(p.Y) || double.IsInfinity(p.Z)))
.Distinct()
.Select(p => fromUnit.Transform(p))
.Select(p => new Point((RoundedDouble)p.X, (RoundedDouble)p.Y, (RoundedDouble)p.Z));
}
}
else
{
// parallel with no intersections
results = empty;
}
}
else
{
// intersection was a line
// find a common point to complete the line then intersect that line with the circles
double x = 0, y = 0, z = 0;
var n = first.Normal;
var m = second.Normal;
var q = first.Center;
var r = second.Center;
if (Math.Abs(lineVector.X) >= MathHelper.Epsilon)
{
x = 0.0;
y = (-m.Z * n.X * q.X - m.Z * n.Y * q.Y - m.Z * n.Z * q.Z + m.X * n.Z * r.X + m.Y * n.Z * r.Y + m.Z * n.Z * r.Z)
/ (-m.Z * n.Y + m.Y * n.Z);
z = (-m.Y * n.X * q.X - m.Y * n.Y * q.Y - m.Y * n.Z * q.Z + m.X * n.Y * r.X + m.Y * n.Y * r.Y + m.Z * n.Y * r.Z)
/ (-m.Z * n.Y + m.Y * n.Z);
}
else if (Math.Abs(lineVector.Y) >= MathHelper.Epsilon)
{
x = (-m.Z * n.X * q.X - m.Z * n.Y * q.Y - m.Z * n.Z * q.Z + m.X * n.Z * r.X + m.Y * n.Z * r.Y + m.Z * n.Z * r.Z)
/ (-m.Z * n.X + m.X * n.Z);
y = 0.0;
z = (-m.X * n.X * q.X - m.X * n.Y * q.Y - m.X * n.Z * q.Z + m.X * n.X * r.X + m.Y * n.X * r.Y + m.Z * n.X * r.Z)
/ (-m.Z * n.X + m.X * n.Z);
}
else if (Math.Abs(lineVector.Z) >= MathHelper.Epsilon)
{
x = (-m.Y * n.X * q.X - m.Y * n.Y * q.Y - m.Y * n.Z * q.Z + m.X * n.Y * r.X + m.Y * n.Y * r.Y + m.Z * n.Y * r.Z)
/ (m.Y * n.X - m.X * n.Y);
y = (-m.X * n.X * q.X - m.X * n.Y * q.Y - m.X * n.Z * q.Z + m.X * n.X * r.X + m.Y * n.X * r.Y + m.Z * n.X * r.Z)
/ (m.Y * n.X - m.X * n.Y);
z = 0.0;
}
else
{
Debug.Assert(false, "zero-vector shouldn't get here");
}
var point = new Point(x, y, z);
var other = point + lineVector;
var intersectionLine = new PrimitiveLine(point, other);
var firstIntersections = intersectionLine.IntersectionPoints(first, false)
.Select(p => new Point((RoundedDouble)p.X, (RoundedDouble)p.Y, (RoundedDouble)p.Z));
var secondIntersections = intersectionLine.IntersectionPoints(second, false)
.Select(p => new Point((RoundedDouble)p.X, (RoundedDouble)p.Y, (RoundedDouble)p.Z));
results = firstIntersections
.Union(secondIntersections)
.Distinct();
}
// verify points are in angle bounds
if (withinBounds)
{
var toFirstUnit = first.FromUnitCircle.Inverse();
var toSecondUnit = second.FromUnitCircle.Inverse();
results = from res in results
// verify point is within first ellipse's angles
let trans1 = toFirstUnit.Transform(res)
let ang1 = ((Vector)trans1).ToAngle()
where first.IsAngleContained(ang1)
// and same for second
let trans2 = toSecondUnit.Transform(res)
let ang2 = ((Vector)trans2).ToAngle()
where second.IsAngleContained(ang2)
select res;
}
return(results);
}
public static IEnumerable <Point> IntersectionPoints(this PrimitiveLine line, PrimitiveEllipse ellipse, bool withinSegment = true)
{
var empty = Enumerable.Empty <Point>();
var l0 = line.P1;
var l = line.P2 - line.P1;
var p0 = ellipse.Center;
var n = ellipse.Normal;
var right = ellipse.MajorAxis.Normalize();
var up = ellipse.Normal.Cross(right).Normalize();
var radiusX = ellipse.MajorAxis.Length;
var radiusY = radiusX * ellipse.MinorAxisRatio;
var transform = ellipse.FromUnitCircle;
var inverse = transform.Inverse();
var denom = l.Dot(n);
var num = (p0 - l0).Dot(n);
var flatPoints = new List <Point>();
if (Math.Abs(denom) < MathHelper.Epsilon)
{
// plane either contains the line entirely or is parallel
if (Math.Abs(num) < MathHelper.Epsilon)
{
// parallel. normalize the plane and find the intersection
var flatLine = line.Transform(inverse);
// the ellipse is now centered at the origin with a radius of 1.
// find the intersection points then reproject
var dv = flatLine.P2 - flatLine.P1;
var dx = dv.X;
var dy = dv.Y;
var dr2 = dx * dx + dy * dy;
var D = flatLine.P1.X * flatLine.P2.Y - flatLine.P2.X * flatLine.P1.Y;
var det = dr2 - D * D;
var points = new List <Point>();
if (det < 0.0 || Math.Abs(dr2) < MathHelper.Epsilon)
{
// no intersection or line is too short
}
else if (Math.Abs(det) < MathHelper.Epsilon)
{
// 1 point
var x = (D * dy) / dr2;
var y = (-D * dx) / dr2;
points.Add(new Point(x, y, 0.0));
}
else
{
// 2 points
var sgn = dy < 0.0 ? -1.0 : 1.0;
var det2 = Math.Sqrt(det);
points.Add(
new Point(
(D * dy + sgn * dx * det2) / dr2,
(-D * dx + Math.Abs(dy) * det2) / dr2,
0.0));
points.Add(
new Point(
(D * dy - sgn * dx * det2) / dr2,
(-D * dx - Math.Abs(dy) * det2) / dr2,
0.0));
}
// ensure the points are within appropriate bounds
if (withinSegment)
{
// line test
points = points.Where(p =>
MathHelper.Between(flatLine.P1.X, flatLine.P2.X, p.X) &&
MathHelper.Between(flatLine.P1.Y, flatLine.P2.Y, p.Y)).ToList();
// circle test
points = points.Where(p => ellipse.IsAngleContained(((Vector)p).ToAngle())).ToList();
}
return(points.Select(p => (Point)transform.Transform(p)));
}
}
else
{
// otherwise line and plane intersect in only 1 point, p
var d = num / denom;
var p = (Point)(l * d + l0);
// verify within the line segment
if (withinSegment && !MathHelper.Between(0.0, 1.0, d))
{
return(empty);
}
// p is the point of intersection. verify if on transformed unit circle
var unitVector = (Vector)inverse.Transform(p);
if (Math.Abs(unitVector.Length - 1.0) < MathHelper.Epsilon)
{
// point is on the unit circle
if (withinSegment)
{
// verify within the angles specified
var angle = Math.Atan2(unitVector.Y, unitVector.X) * MathHelper.RadiansToDegrees;
if (MathHelper.Between(ellipse.StartAngle, ellipse.EndAngle, angle.CorrectAngleDegrees()))
{
return(new[] { p });
}
}
else
{
return(new[] { p });
}
}
}
// point is not on unit circle
return(empty);
}
