internal static void Sweep(IEnumerable<Polyline> obstacles,
Point direction, double coneAngle, VisibilityGraph visibilityGraph,
IEnumerable<Point> portLocations) {
var cs = new LineSweeperForPortLocations(obstacles, direction, direction.Rotate(-coneAngle/2),
direction.Rotate(coneAngle/2), visibilityGraph, portLocations);
cs.Calculate();
}
void Rotate(double x, double y, double x1, double y1, double r1,
double x2, double y2, double r2,
double x3, double y3, double r3)
{
Point p1 = new Point(x1, y1), p2 = new Point(x2, y2), p3 = new Point(x3, y3);
Point p = new Point(x, y);
// 1 2 3
Expect(p, p1, r1, p2, r2, p3, r3);
// 2 1 3
Expect(p, p2, r2, p1, r1, p3, r3);
// 3 2 1
Expect(p, p3, r3, p2, r2, p1, r1);
// Heavy random translation + rotations
for (int i = 0; i < 50; i++)
{
double O = random.NextDouble() * 2 * Math.PI;
Point T = new Point(random.NextDouble() * 1000 - 500, random.NextDouble() * 2000 - 1000);
Expect((p+T).Rotate(O), (p1+T).Rotate(O), r1, (p2+T).Rotate(O), r2, (p3+T).Rotate(O), r3);
Expect(p.Rotate(O), p1.Rotate(O), r1, p2.Rotate(O), r2, p3.Rotate(O), r3);
}
}
public Point IntersectCircles(Point c1, double r1, Point c2, double r2, Point c3, double r3)
{
// To compute the intersection of the 3 circle, we intersect two of them, get 2
// solutions, and see which one is on the third circle.
// Step 1: Translate all circle centers so that c1 = (0, 0).
c2 = c2 - c1;
c3 = c3 - c1;
// Step 2: Rotate all around (0, 0) so that c2 is now on Ox axis.
double O = Math.Atan2(c2.Y, c2.X);
c2 = c2.Rotate(-O);
c3 = c3.Rotate(-O);
// Step 3: Intersect circles (0, 0, r1) with (c2.X, 0, r2)
Point p1 = new Point(), p2 = new Point(), sol;
p1.X = p2.X = (r1 * r1 - r2 * r2 + c2.X * c2.X) / (2 * c2.X);
// sqrt( negative zero ) = NaN
if (r1 > p1.X) p1.Y = Math.Sqrt(r1 * r1 - p1.X * p1.X);
else p1.Y = 0;
p2.Y = - p1.Y;
if (Util.IsEqual(p2.DistanceTo(c3), r3))
sol = p2;
else
sol = p1;
// Unapply step 2
sol = sol.Rotate(O);
// Unapply step 1
sol = sol + c1;
return sol;
}
private static Point GetSearchDirection(Point derivative, double side)
{
Point direction = derivative.Rotate(Math.PI / 2).Normalize() * side;
// Rotating can cause tiny drift in the X/Y values. Round so that 0 actually equals 0
direction = new Point(Math.Round(direction.X, 5), Math.Round(direction.Y, 5));
return direction;
}
/// <summary>
/// Creates a spline between two nodes big enough to draw arrowheads
/// </summary>
/// <param name="edge"></param>
public static void CreateBigEnoughSpline(Edge edge)
{
ValidateArg.IsNotNull(edge, "edge");
Point a = edge.Source.Center;
Point b = edge.Target.Center;
Point bMinA = b - a;
double l = bMinA.Length;
Point perp;
if (l < 0.001)
{
perp = new Point(1, 0);
b = a + perp.Rotate(Math.PI / 2);
}
else
{
perp = bMinA.Rotate(Math.PI / 2);
}
double maxArrowLength = 1;
if (edge.EdgeGeometry.SourceArrowhead != null)
{
maxArrowLength += edge.EdgeGeometry.SourceArrowhead.Length;
}
if (edge.EdgeGeometry.TargetArrowhead != null)
{
maxArrowLength += edge.EdgeGeometry.TargetArrowhead.Length;
}
perp = perp.Normalize() * 1.5 * maxArrowLength;
int i = 1;
do
{
CubicBezierSegment seg = Curve.CreateBezierSeg(a, b, perp, i);
if (TrimSplineAndCalculateArrowheads(edge.EdgeGeometry, edge.Source.BoundaryCurve,
edge.Target.BoundaryCurve,
seg, false, false))
{
break;
}
i *= 2;
const int stop = 10000;
if (i >= stop)
{
CreateEdgeCurveWithNoTrimming(edge, a, b);
return;
}
} while (true);
}
void ExtendPolyline(Point tangentAtIntersection, IntersectionInfo x, Point polylineTangent, HookUpAnywhereFromInsidePort port) {
var normal=tangentAtIntersection.Rotate(Math.PI/2);
if(normal*polylineTangent<0)
normal=-normal;
var pointBeforeLast = x.IntersectionPoint + normal * port.HookSize;
Point pointAfterX;
if (!Point.LineLineIntersection(pointBeforeLast, pointBeforeLast+tangentAtIntersection, _polyline.End, _polyline.End+polylineTangent, out pointAfterX))
return;
_polyline.AddPoint(pointAfterX);
_polyline.AddPoint(pointBeforeLast);
_polyline.AddPoint(x.IntersectionPoint);
}
/// <summary>
/// p1 and p2 represent the closest feature. Two cases are possible p1=p2, or p1 and p2 share an edge going from p1 to p2
/// Remind that the polygons are oriented clockwise
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="bisectorPivot"></param>
/// <param name="bisectorRay"></param>
/// <returns></returns>
internal int FindTheFurthestVertexFromBisector(int p1, int p2, Point bisectorPivot, Point bisectorRay) {
Point directionToTheHill = bisectorRay.Rotate(Math.PI / 2);
if ((polyline.StartPoint.Point - bisectorPivot) * directionToTheHill < 0)
directionToTheHill = -directionToTheHill;
if (p1 == p2)
p2 = Next(p1);
//binary search
do {
int m = Median(p2, p1); //now the chunk goes clockwise from p2 to p1
Point mp = Pnt(m);
if ((Pnt(Next(m)) - mp) * directionToTheHill >= 0)
p2 = Next(m);
else if ((Pnt(Prev(m)) - mp) * directionToTheHill >= 0)
p1 = Prev(m);
else
p1 = p2 = m;
}
while (p1 != p2);
return p1;
}
public void RotateTest1()
{
var p1 = new Point (1, 0);
var p2 = new Point (0, 0);
p1.Rotate (p2, Math.PI / 2);
TestHelper.AlmostEqual (p1.X, 0);
TestHelper.AlmostEqual (p1.Y, 1);
}
