Fill a target area found by trilateration in C#

[trilateration]

The post Perform trilateration in C# explains how a C# program can use trilateration to find an area where a target point lies. This post explains how you can draw that area.


The Target Area

[trilateration]

The target area found by trilateration is similar to a triangle except each of its sides is a circular arc. The picture on the right shows three circles with the target area found by trilateration drawn in bold arcs.

To draw that area, we must solve two problems. First, we must find the arcs. Second, we must arrange the arcs so they are joined end-to-end in the right order.

Finding Arcs

To keep track of the arcs that define the target area, this example uses a new ArcInfo class. The following code shows the class declaration, its fields, and its constructor. (I’ll show the rest shortly.)

public class ArcInfo
{
    public PointF Point1, Point2, Center;
    public RectangleF Rect;
    public float StartAngle, SweepAngle;

    public ArcInfo(PointF center, PointF point1, PointF point2)
    {
        Center = center;
        Point1 = point1;
        Point2 = point2;

        float dx1 = Point1.X - Center.X;
        float dy1 = Point1.Y - Center.Y;
        float dx2 = Point2.X - Center.X;
        float dy2 = Point2.Y - Center.Y;

        // Get the radius and rectangle.
        float radius = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1);
        Rect = new RectangleF(
            Center.X - radius,
            Center.Y - radius,
            2 * radius, 2 * radius);

        // Calculate the angles.
        double angle1 = Math.Atan2(dy1, dx1);
        double angle2 = Math.Atan2(dy2, dx2);

        // Make angle2 > angle1.
        if (angle1 > angle2) angle2 += 2 * Math.PI;

        // If the sweep is more thn 180 degrees, swap them.
        double sweep = angle2 - angle1;
        if (sweep > Math.PI)
        {
            Swap(ref angle1, ref angle2);
            Swap(ref Point1, ref Point2);
            angle2 += 2 * Math.PI;
            sweep = angle2 - angle1;
        }

        StartAngle = (float)(angle1 * 180 / Math.PI);
        SweepAngle = (float)((angle2 - angle1) * 180 / Math.PI);
    }
    ...
}

The trilateration code described in the earlier post finds the points at the ends of the arcs that define the target area. The ArcInfo class stores those points in its Point1 and Point2 fields. It also stores the center of the arc’s circle in the Center field.

To draw an arc in .NET, we need a bounding rectangle, a start angle, and a sweep angle. The ArcInfo class stores those values in its Rect, StartAngle, and SweepAngle fields.

The class’s constructor takes as parameters the circle’s center and the two points at the ends of the arc. The code saves those values and then calculates the horizontal and vertical (dx and dy) differences between those points and the circle’s center.

Next the code finds the distance between the circle’s center and the first arc end point to get the circle’s radius. From the center point and the radius, the code defines the circle’s bounding rectangle.

The constructor then uses the two points’ dx and dy values and the Math.Atan2 function to calculate the points’ angles with respect to the center point.

Note that Math.Atan returns an angle in radians, but the .NET arc drawing method uses degrees. This code works in radians for a while but then converts the results to degrees so drawing is easier later.

For definiteness, I want the class to store the points so the arc from Point1 to Point2 is clockwise and the smaller of the two possible arcs so it spans no more than π radians (180 degrees). To achieve those goals, the code first guarantees that angle1 <= angle2. If that is not initially true, the code adds 2 π radians (360 degrees) to angle2. After that angle2 must be larger than angle1.

Now that angle1 < angle2, we know that the arc from Point1 to Point2 is clockwise, but we don’t know if the arc is the larger or smaller arc between the two points. To find out whether or not this is a large arc, the code subtracts the two angles to get the sweep angle and then tests whether it is greater than π radians (180 degrees).

If the sweep angle is greater than π radians, then we want the arc to sweep from point2 to point1. That will give us a clockwise arc that is no larger than π radians.

To do that, we swap the two angles. The code uses the Swap helper method described shortly to interchange the two angles and the two points. We originally adjusted the angles if necessary so the second angle was greater than the first. After swapping those values, that is no longer true. To make that true again, so the arc sweeps clockwise, the code adds 2 π to the new value of angle2 so that it is greater than angle1. The program then recalculates the sweep angle for the new angles.

Finally, the constructor saves the start and sweep angles in fields StartAngle and SweepAngle.

The following code shows the Swap helper method.

public void Swap<T>(ref T a, ref T b)
{
    T temp = a;
    a = b;
    b = temp;
}

This method simply uses a temporary variable to swap two values. The only interesting thing about it is that it uses a type parameter so it can swap two items of just about any type including float and PointF values.

The ArcInfo class’s final method, Draw, uses the following code to draw the arc.

public void Draw(Graphics gr, Pen pen)
{
    gr.DrawArc(pen, Rect, StartAngle, SweepAngle);
}

This method simply calls a Graphics object’s DrawArc method, passing it the arc’s bounding rectangle, start angle, and sweep angle.

Arranging Arcs

After the program has found the three arcs that define the target area, it needs to arrange them so they bound the area. To do that correctly, the arcs must all be oriented in the same direction, either clockwise or counterclockwise. The ArcInfo class automatically orients arcs so they are clockwise, so that part is done.

[trilateration]

Now we just need to arrange the arcs so they connect end-to-end in the right order. This is fairly easy, but you can’t just assume that any order will do. If you do this incorrectly, you’ll get the arrangement shown in the picture on the right. If you use the incorrectly ordered arcs to build a GraphicsPath and then fill the path, it will look normal because the fill mode used by the FillPath method fills all of the area inside the arcs. However, if you draw the path instead of filling it, you’ll see the odd shape shown in the picture.

The program orders the arcs in its Trilaterate method, which is shown below. Most of that method is similar to the version used by the previous example. The new code is highlighted in blue.

// Trilaterate.
// Throw an exception if there is a problem.
private PointF[] Trilaterate(RectangleF circle1,
    RectangleF circle2, RectangleF circle3,
    out ArcInfo[] arc_infos)
{
    // Convert the circles from bounding rectangles
    // to centers and radii.
    float cx1 = circle1.X + circle1.Width / 2f;
    float cy1 = circle1.Y + circle1.Height / 2f;
    float cx2 = circle2.X + circle2.Width / 2f;
    float cy2 = circle2.Y + circle2.Height / 2f;
    float cx3 = circle3.X + circle3.Width / 2f;
    float cy3 = circle3.Y + circle3.Height / 2f;
    float r1 = circle1.Width / 2f;
    float r2 = circle2.Width / 2f;
    float r3 = circle3.Width / 2f;

    // Find the points of intersection.
    PointF
        intersection12a, intersection12b,
        intersection23a, intersection23b,
        intersection31a, intersection31b;
    if (FindCircleCircleIntersections(
            cx1, cy1, r1, cx2, cy2, r2,
            out intersection12a, out intersection12b) == 0)
        throw new Exception("circle1 and circle2 do not intersect.");
    if (FindCircleCircleIntersections(
            cx2, cy2, r2, cx3, cy3, r3,
            out intersection23a, out intersection23b) == 0)
        throw new Exception("circle2 and circle3 do not intersect.");
    if (FindCircleCircleIntersections(
            cx3, cy3, r3, cx1, cy1, r1,
            out intersection31a, out intersection31b) == 0)
        throw new Exception("circle3 and circle1 do not intersect.");

    // Find the points that make up the target area.
    PointF[] triangle = new PointF[3];
    PointF center1 = new PointF(cx1, cy1);
    PointF center2 = new PointF(cx2, cy2);
    PointF center3 = new PointF(cx3, cy3);
    if (Distance(intersection12a, center3) <
        Distance(intersection12b, center3))
            triangle[0] = intersection12a;
    else
        triangle[0] = intersection12b;
    if (Distance(intersection23a, center1) <
        Distance(intersection23b, center1))
            triangle[1] = intersection23a;
    else
        triangle[1] = intersection23b;
    if (Distance(intersection31a, center2) <
        Distance(intersection31b, center2))
            triangle[2] = intersection31a;
    else
        triangle[2] = intersection31b;

    // Make ArcInfo data.
    ArcInfo arc_infos1 = new ArcInfo(
        center1,
        triangle[0],
        triangle[2]);
    ArcInfo arc_infos2 = new ArcInfo(
        center2,
        triangle[0],
        triangle[1]);
    ArcInfo arc_infos3 = new ArcInfo(
        center3,
        triangle[1],
        triangle[2]);

    // Order the ArcInfo data.
    arc_infos = new ArcInfo[3];
    arc_infos[0] = arc_infos1;
    if (arc_infos1.Point2 == arc_infos2.Point1)
    {
        arc_infos[1] = arc_infos2;
        arc_infos[2] = arc_infos3;
    }
    else
    {
        arc_infos[1] = arc_infos3;
        arc_infos[2] = arc_infos2;
    }

    return triangle;
}

This method finds the points where the three circles intersect as before. For each pair of intersections, it determines which point is closer to the remaining circle and adds that closer point to the triangle array.

That defines the triangular target area. Now we just need to convert the triangle into arc data.

The program creates three ArcInfo objects to define the sides of the target area. The ArcInfo class constructor arranges each object’s Point1 and Point2 values so the arcs are oriented clockwise.

Next the code creates a new arc_infos array to hold the ArcInfo objects in their proper order and adds the first arc to the array.

The method then compares the first arc’s end point to the second arc’s starting point. If they are the same point, then the second arc should follow the first. In that case, the code adds the second arc to the arc_infos array’s second position and places the third arc in the third position.

If the first arc does not end where the second arc begins, then the code places the third arc in the arc_infos array’s second position and puts the second arc in the last position.

The method finishes by returning the triangle that defines the target area. At this point you might not even need that triangle any more because the arc_infos array defines the target area more precisely. You can even use the ArcInfo objects’ Point1 values to retrieve the corners of the triangle if you want them. I left the method as it is to minimize changes.

Building the Path

The last really interesting piece of this example is the following MakeArcPath method, which uses the ArcInfo objects to create a GraphicsPath.

// Make the arc path.
private GraphicsPath MakeArcPath(ArcInfo[] arc_infos)
{
    GraphicsPath path = new GraphicsPath();

    foreach (ArcInfo arc_info in arc_infos)
    {
        path.AddArc(arc_info.Rect,
            arc_info.StartAngle, arc_info.SweepAngle);
    }
    return path;
}

This method creates a new GraphicsPath object. It then loops through the arcs in the arc_infos array and adds each of them to the path. The method finishes by returning the path.

Later the program’s Paint event handler fills the target area by calling the Graphics object’s FillPath method, passing it the path saved in the variable ArcPath.

// Fill the area.
e.Graphics.FillPath(Brushes.Pink, ArcPath);

Conclusion

This method seems to work fairly well. If the three circles intersect, the trilateration finds the target area and the program creates a GraphicsPath to draw that area. There are a lot of other details that this post doesn’t cover, such as allowing the user to select the three circles, and drawing various circles, arcs, and points of intersection. See the earlier post and download this example program to see additional details.


Download Example   Follow me on Twitter   RSS feed   Donate




About RodStephens

Rod Stephens is a software consultant and author who has written more than 30 books and 250 magazine articles covering C#, Visual Basic, Visual Basic for Applications, Delphi, and Java.
This entry was posted in algorithms, geometry, mathematics and tagged , , , , , , , , . Bookmark the permalink.

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.