+
+Vector Geometry::GetNormalOfPointAndLine(Point p, Line * l)
+{
+ Vector normal = Vector::Normal(l->p[0], l->p[1]);
+ normal.Unit();
+ double dir = Vector::Dot(p - l->p[0], normal);
+
+ if (dir < 0)
+ normal = -normal;
+
+ 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 = (tail + head) / 2.0;
+ 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);
+}