Preliminary support for Polylines.

[architektonas] / src / geometry.cpp

diff --git a/src/geometry.cpp b/src/geometry.cpp
index 055c581a652a7505e9a42a97c220649385ccfe30..6b3935db57895998a63026dcb592c7b604229260 100644 (file)
--- a/src/geometry.cpp
+++ b/src/geometry.cpp
@@ -524,3 +524,100 @@ Vector Geometry::GetNormalOfPointAndLine(Point p, Line * l)
 
        return normal;
 }
+
+Circle Geometry::FindCircleForThreePoints(Point p1, Point p2, Point p3)
+{
+       // We use matrices and determinants to find the center and radius of the
+       // circle, given the three points passed in:
+       //
+       // [(x^2 + y^2)    x   y   1 ]
+       // [(x1^2 + y1^2)  x1  y1  1 ]
+       // [(x2^2 + y2^2)  x2  y2  1 ]
+       // [(x3^2 + y3^2)  x3  y3  1 ]
+       //
+       // Then, center <x0, y0> is found by:
+       //
+       // x0 = 1/2 • M12/M11, y0 = -1/2 • M13/M11
+       //
+       // The radius can be found with the center and any of the points via
+       // Pythagoras, or with:
+       //
+       // r^2 = x0^2 + y0^2 + M14/M11
+       //
+       // If M11 = 0, then the three points are colinear and there is no circle.
+       // (M## is the minor determinant of the 4x4 matrix, where a 3x3 matrix is
+       // created by deleting the row and column of the 4x4 with the indices
+       // given.)
+
+       Circle c;
+       double m[3][4];
+
+       m[0][0] = (p1.x * p1.x) + (p1.y * p1.y);
+       m[0][1] = p1.x;
+       m[0][2] = p1.y;
+       m[0][3] = 1.0;
+       m[1][0] = (p2.x * p2.x) + (p2.y * p2.y);
+       m[1][1] = p2.x;
+       m[1][2] = p2.y;
+       m[1][3] = 1.0;
+       m[2][0] = (p3.x * p3.x) + (p3.y * p3.y);
+       m[2][1] = p3.x;
+       m[2][2] = p3.y;
+       m[2][3] = 1.0;
+
+       double minor11 = Determinant3x3(m[0][1], m[0][2], m[0][3], m[1][1], m[1][2], m[1][3], m[2][1], m[2][2], m[2][3]);
+
+       // Sanity check: if M11 is zero, the points are colinear.
+       if (minor11 == 0)
+               return c;
+
+       double minor12 = Determinant3x3(m[0][0], m[0][2], m[0][3], m[1][0], m[1][2], m[1][3], m[2][0], m[2][2], m[2][3]);
+       double minor13 = Determinant3x3(m[0][0], m[0][1], m[0][3], m[1][0], m[1][1], m[1][3], m[2][0], m[2][1], m[2][3]);
+       double minor14 = Determinant3x3(m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1], m[2][2]);
+
+       c.p[0].x = 0.5 * (minor12 / minor11);
+       c.p[0].y = -0.5 * (minor13 / minor11);
+       c.radius[0] = sqrt((c.p[0].x * c.p[0].x) + (c.p[0].y * c.p[0].y) + (minor14 / minor11));
+
+       return c;
+}
+
+double Geometry::Determinant3x3(double a11, double a12, double a13, double a21, double a22, double a23, double a31, double a32, double a33)
+{
+       return (a11 * ((a22 * a33) - (a32 * a23)))
+               - (a12 * ((a21 * a33) - (a31 * a23)))
+               + (a13 * ((a21 * a32) - (a31 * a22)));
+}
+
+Arc Geometry::Unpack(Point tail, Point head, double bump)
+{
+       double length = Vector::Magnitude(tail, head) / 2.0;
+       double bumpLen = length * fabs(bump);
+       Point midpoint = Vector::Midpoint(tail, head);
+       Vector mpNormal = Vector::Normal(tail, head); // Normal points to the left
+
+       // Flip the normal if the bump is pointing left
+       if (bump < 0)
+               mpNormal = -mpNormal;
+
+       // N.B.: The radius can also be found with r = (a² + h²) / 2h where a is
+       // the 1/2 length of the line segment and h is the bump length.
+//     double radius = (length + (1.0 / length)) / 2.0;
+       double radius = 0.5 * (((length * length) + (bumpLen * bumpLen)) / bumpLen);
+       Vector ctrVec = mpNormal * (radius - bumpLen);
+       Point center = midpoint + ctrVec;
+
+       // Find arc endpoints...
+       double angle1 = Vector::Angle(center, tail);
+       double angle2 = Vector::Angle(center, head);
+       double span = angle2 - angle1;
+
+       // Fix span depending on which way the arc is being drawn...
+       if (bump > 0 && span < 0)
+               span += TAU;
+
+       if (bump < 0 && span > 0)
+               span -= TAU;
+
+       return Arc(center, radius, angle1, span);
+}