More polyline upgrading: points and arcs are now editable with the GUI.

[architektonas] / src / geometry.cpp

//
// geometry.cpp: Algebraic geometry helper functions
//
// Part of the Architektonas Project
// (C) 2011 Underground Software
// See the README and GPLv3 files for licensing and warranty information
//
// JLH = James Hammons <jlhamm@acm.org>
//
// WHO  WHEN        WHAT
// ---  ----------  ------------------------------------------------------------
// JLH  08/31/2011  Created this file
//
// NOTE: All methods in this class are static.
//

#include "geometry.h"
#include <math.h>
#include <stdio.h>
#include "global.h"
#include "mathconstants.h"

// Returns the parameter of a point in space to this vector. If the parameter
// is between 0 and 1, the normal of the vector to the point is on the vector.
// Note: lp1 is the tail, lp2 is the head of the line (vector).
double Geometry::ParameterOfLineAndPoint(Point tail, Point head, Point point)
{
        // Geometric interpretation:
        // The parameterized point on the vector lineSegment is where the normal of
        // the lineSegment to the point intersects lineSegment. If the pp < 0, then
        // the perpendicular lies beyond the 1st endpoint. If pp > 1, then the
        // perpendicular lies beyond the 2nd endpoint.

        Vector lineSegment = head - tail;
        double magnitude = lineSegment.Magnitude();
        Vector pointSegment = point - tail;
        double t = lineSegment.Dot(pointSegment) / (magnitude * magnitude);

        return t;
}

double Geometry::DistanceToLineFromPoint(Point tail, Point head, Point point)
{
        // Interpretation: given a line in the form x = a + tu, where u is the
        // unit vector of the line, a is the tail and t is a parameter which
        // describes the line, the distance of a point p to the line is given by:
        // || (a - p) - ((a - p) • u) u ||
        // We go an extra step: we set the sign to reflect which side of the line
        // it's on (+ == to the left if head points away from you, - == to the
        // right)
        Vector line(tail, head);
        Vector u = line.Unit();
        Vector a_p = tail - point;
        Vector dist = a_p - (u * (a_p).Dot(u));

        double angle = Vector::Angle(tail, point) - line.Angle();

        if (angle < 0)
                angle += TAU;

        return dist.Magnitude() * (angle < HALF_TAU ? +1.0 : -1.0);
}

Point Geometry::MirrorPointAroundLine(Point point, Point tail, Point head)
{
        // Get the vector of the intersection of the line and the normal on the
        // line to the point in question.
        double t = ParameterOfLineAndPoint(tail, head, point);
        Vector v = Vector(tail, head) * t;

        // Get the point normal to point to the line passed in
        Point normalOnLine = tail + v;

        // Make our mirrored vector (head - tail)
        Vector mirror = -(point - normalOnLine);

        // Find the mirrored point
        Point mirroredPoint = normalOnLine + mirror;

        return mirroredPoint;
}

//
// point: The point we're rotating
// rotationPoint: The point we're rotating around
//
Point Geometry::RotatePointAroundPoint(Point point, Point rotationPoint, double angle)
{
        Vector v = Vector(rotationPoint, point);
        double px = (v.x * cos(angle)) - (v.y * sin(angle));
        double py = (v.x * sin(angle)) + (v.y * cos(angle));

        return Vector(rotationPoint.x + px, rotationPoint.y + py, 0);
}

double Geometry::Determinant(Point p1, Point p2)
{
        return (p1.x * p2.y) - (p2.x * p1.y);
}

void Geometry::Intersects(Object * obj1, Object * obj2)
{
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        if ((obj1->type == OTLine) && (obj2->type == OTLine))
                CheckLineToLineIntersection(obj1, obj2);
        else if ((obj1->type == OTCircle) && (obj2->type == OTCircle))
                CheckCircleToCircleIntersection(obj1, obj2);
        else if ((obj1->type == OTLine) && (obj2->type == OTCircle))
                CheckLineToCircleIntersection(obj1, obj2);
        else if ((obj1->type == OTCircle) && (obj2->type == OTLine))
                CheckLineToCircleIntersection(obj2, obj1);
}

/*
Intersecting line segments:
An easier way:
Segment L1 has edges A=(a1,a2), A'=(a1',a2').
Segment L2 has edges B=(b1,b2), B'=(b1',b2').
Segment L1 is the set of points tA'+(1-t)A, where 0<=t<=1.
Segment L2 is the set of points sB'+(1-s)B, where 0<=s<=1.
Segment L1 meet segment L2 if and only if for some t and s we have
tA'+(1-t)A=sB'+(1-s)B
The solution of this with respect to t and s is

t=((-b?'a?+b?'b?+b?a?+a?b?'-a?b?-b?b?')/(b?'a?'-b?'a?-b?a?'+b?a?-a?'b?'+a?'b?+a?b?'-a?b?))

s=((-a?b?+a?'b?-a?a?'+b?a?+a?'a?-b?a?')/(b?'a?'-b?'a?-b?a?'+b?a?-a?'b??+a?'b?+a?b?'-a?b?))

So check if the above two numbers are both >=0 and <=1.
*/


// Finds the intersection between two lines (if any)
void Geometry::CheckLineToLineIntersection(Object * l1, Object * l2)
{
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        Vector r(l1->p[0], l1->p[1]);
        Vector s(l2->p[0], l2->p[1]);
        Vector v1 = l2->p[0] - l1->p[0];        // q - p

        double rxs = (r.x * s.y) - (s.x * r.y);
        double t, u;

        // The angle is zero, so the lines probably overlap, or are parallel.  Either way, there is either INFINITE intersections, or zero.  Either way, the result is useless to us.  It does present a bit of an inconsistency though: lines connected at their endpoints that aren't at a zero angle will show as overlapping (the only case this could even REMOTELY be useful is when the angle between is 180°--not zero, as zero is the degenerate case).  :-P
        if (rxs == 0)
        {
// WHY would you do this???  The lines overlap at an INFINITE number of points!
// The assumption is that the angle is 180°, not 0°.
                // If we want to detect the degenerate case, we can check the parameter of the endpoints of the one line to the other.  If any of the parameters are in (0, 1), then it's the degenerate case (we would check the endpoints of the shorter segment against the longer).
/*              double qpxr = (v1.x * r.y) - (r.x * v1.y);

                // Line segments are parallel, so no intersection...
                if (qpxr != 0)
                        return;

                // Otherwise, the segments are colinear.  Need to check for the 0° (degenerate) vs the 180° (OK) case.
                Object * larger = l1;
                Object * smaller = l2;

                if (r->Magnitude() < s->Magnitude())
                {
                        larger = l2;
                        smaller = l1;
                }

                double param1 = ParameterOfLineAndPoint(larger->p[0], larger->p[1], smaller->p[0]);
                double param2 = ParameterOfLineAndPoint(larger->p[0], larger->p[1], smaller->p[1]);

                // Check for the degenerate case, and return if found
                if ((param1 > 0 && param1 < 1.0) || (param2 > 0 && param2 < 1.0))
                        return;

////    or just use AngleBetween: Nah, won't work...

                // Check to see which endpoints are connected... Four possibilities:
                if (l1->p[0] == l2->p[0])
                        t = 0, u = 0;
                else if (l1->p[0] == l2->p[1])
                        t = 0, u = 1.0;
                else if (l1->p[1] == l2->p[0])
                        t = 1.0, u = 0;
                else if (l1->p[1] == l2->p[1])
                        t = 1.0, u = 1.0;
                else*/
                        return;
        }
        else
        {
                t = ((v1.x * s.y) - (s.x * v1.y)) / rxs;
                u = ((v1.x * r.y) - (r.x * v1.y)) / rxs;
        }

        // Check that the parameters are above the epsilon, otherwise clamp them to
        // zero or one, as the case may be
        if (fabs(t) < EPSILON)
                t = 0;
        else if (fabs(1.0 - t) < EPSILON)
                t = 1.0;

        if (fabs(u) < EPSILON)
                u = 0;
        else if (fabs(1.0 - u) < EPSILON)
                u = 1.0;

        Global::intersectParam[0] = t;
        Global::intersectParam[1] = u;

        // If the parameters are in range, we have overlap!
        if ((t >= 0) && (t <= 1.0) && (u >= 0) && (u <= 1.0))
                Global::numIntersectParams = 2;
}

void Geometry::CheckCircleToCircleIntersection(Object * c1, Object * c2)
{
        // Set up global vars
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        // Get the distance between the centers of the circles
        Vector centerLine(c1->p[0], c2->p[0]);
        double d = centerLine.Magnitude();
        double clAngle = centerLine.Angle();

        // If the distance between centers is greater than the sum of the radii or
        // less than the difference between the radii, there is NO intersection
        if ((d > (c1->radius[0] + c2->radius[0]))
                || (d < fabs(c1->radius[0] - c2->radius[0])))
                return;

        // If the distance between centers is equal to the sum of the radii or
        // equal to the difference between the radii, the intersection is tangent
        // to both circles.
        if (d == (c1->radius[0] + c2->radius[0]))
        {
                Global::intersectPoint[0].x = c1->p[0].x + (cos(clAngle) * c1->radius[0]);
                Global::intersectPoint[0].y = c1->p[0].y + (sin(clAngle) * c1->radius[0]);
                Global::numIntersectPoints = 1;
                return;
        }
        else if (d == fabs(c1->radius[0] - c2->radius[0]))
        {
                double sign = (c1->radius[0] > c2->radius[0] ? +1 : -1);
                Global::intersectPoint[0].x = c1->p[0].x + (cos(clAngle) * c1->radius[0] * sign);
                Global::intersectPoint[0].y = c1->p[0].y + (sin(clAngle) * c1->radius[0] * sign);
                Global::numIntersectPoints = 1;
                return;
        }

/*
       c² = a² + b² - 2ab·cos µ
2ab·cos µ = a² + b² - c²
    cos µ = (a² + b² - c²) / 2ab
*/
        // Use the Law of Cosines to find the angle between the centerline and the
        // radial line on Circle #1
        double a = acos(((c1->radius[0] * c1->radius[0]) + (d * d) - (c2->radius[0] * c2->radius[0])) / (2.0 * c1->radius[0] * d));

        // Finally, find the points of intersection by using +/- the angle found
        // from the centerline's angle
        Global::intersectPoint[0].x = c1->p[0].x + (cos(clAngle + a) * c1->radius[0]);
        Global::intersectPoint[0].y = c1->p[0].y + (sin(clAngle + a) * c1->radius[0]);
        Global::intersectPoint[1].x = c1->p[0].x + (cos(clAngle - a) * c1->radius[0]);
        Global::intersectPoint[1].y = c1->p[0].y + (sin(clAngle - a) * c1->radius[0]);
        Global::numIntersectPoints = 2;
}

//
// N.B.: l is the line, c is the circle
//
void Geometry::CheckLineToCircleIntersection(Object * l, Object * c)
{
        // Set up global vars
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        // Step 1: Find shortest distance from center of circle to the infinite line
        double t = ParameterOfLineAndPoint(l->p[0], l->p[1], c->p[0]);
        Point p = l->p[0] + (Vector(l->p[0], l->p[1]) * t);
        Vector radial = Vector(c->p[0], p);
        double distance = radial.Magnitude();

        // Step 2: See if we have 0, 1, or 2 intersection points

        // Case #1: No intersection points
        if (distance > c->radius[0])
                return;
        // Case #2: One intersection point (possibly--tangent)
        else if (distance == c->radius[0])
        {
                // Only intersects if the parameter is on the line segment!
                if ((t >= 0.0) && (t <= 1.0))
                {
                        Global::intersectPoint[0] = c->p[0] + radial;
                        Global::numIntersectPoints = 1;
                }

                return;
        }

        // Case #3: Two intersection points (possibly--secant)

        // So, we have the line, and the perpendicular from the center of the
        // circle to the line. Now figure out where the intersection points are.
        // This is a right triangle, though do we really know all the sides?
        // Don't need to, 2 is enough for Pythagoras :-)
        // Radius is the hypotenuse, so we have to use c² = a² + b² => a² = c² - b²
        double perpendicularLength = sqrt((c->radius[0] * c->radius[0]) - (distance * distance));

        // Now, find the intersection points using the length...
        Vector lineUnit = Vector(l->p[0], l->p[1]).Unit();
        Point i1 = p + (lineUnit * perpendicularLength);
        Point i2 = p - (lineUnit * perpendicularLength);

        // Next we need to see if they are on the line segment...
        double u = ParameterOfLineAndPoint(l->p[0], l->p[1], i1);
        double v = ParameterOfLineAndPoint(l->p[0], l->p[1], i2);

        if ((u >= 0.0) && (u <= 1.0))
        {
                Global::intersectPoint[Global::numIntersectPoints] = i1;
                Global::numIntersectPoints++;
        }

        if ((v >= 0.0) && (v <= 1.0))
        {
                Global::intersectPoint[Global::numIntersectPoints] = i2;
                Global::numIntersectPoints++;
        }
}

// should we just do common trig solves, like AAS, ASA, SAS, SSA?
// Law of Cosines:
// c² = a² + b² - 2ab * cos(C)
// Solving for C:
// cos(C) = (c² - a² - b²) / -2ab = (a² + b² - c²) / 2ab
// Law of Sines:
// a / sin A = b / sin B = c / sin C

// Solve the angles of the triangle given the sides. Angles returned are
// opposite of the given sides (so a1 consists of sides s2 & s3, and so on).
void Geometry::FindAnglesForSides(double s1, double s2, double s3, double * a1, double * a2, double * a3)
{
        // Use law of cosines to find 1st angle
        double cosine1 = ((s2 * s2) + (s3 * s3) - (s1 * s1)) / (2.0 * s2 * s3);

        // Check for a valid triangle
        if ((cosine1 < -1.0) || (cosine1 > 1.0))
                return;

        double angle1 = acos(cosine1);

        // Use law of sines to find 2nd & 3rd angles
// sin A / a = sin B / b
// sin B = (sin A / a) * b
// B = arcsin( sin A * (b / a))
// ??? ==> B = A * arcsin(b / a)
/*
Well, look here:
sin B = sin A * (b / a)
sin B / sin A = b / a
arcsin( sin B / sin A ) = arcsin( b / a )

hmm... dunno...
*/

        double angle2 = asin(s2 * (sin(angle1) / s1));
        double angle3 = asin(s3 * (sin(angle1) / s1));

        if (a1)
                *a1 = angle1;

        if (a2)
                *a2 = angle2;

        if (a3)
                *a3 = angle3;
}

Point Geometry::GetPointForParameter(Object * obj, double t)
{
        if (obj->type == OTLine)
        {
                // Translate line vector to the origin, then add the scaled vector to
                // initial point of the line.
                Vector v = obj->p[1] - obj->p[0];
                return obj->p[0] + (v * t);
        }

        return Point(0, 0);
}

Point Geometry::Midpoint(Line * line)
{
        return Point((line->p[0].x + line->p[1].x) / 2.0,
                (line->p[0].y + line->p[1].y) / 2.0);
}

/*
How to find the tangent of a point off a circle:

 •  Calculate the midpoint of the point and the center of the circle
 •  Get the length of the line segment from point and the center divided by two
 •  Use that length to construct a circle with the point at the center and the
    radius equal to that length
 •  The intersection of the two circles are the tangent points

Another way:

 •  Find angle between radius and line between point and center
 •  Angle +/- from line (point to center) are the tangent points

*/
void Geometry::FindTangents(Object * c, Point p)
{
        // Set up global vars
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        // Get the distance between the point and the center of the circle, the
        // length, and the angle.
        Vector centerLine(c->p[0], p);
        double d = centerLine.Magnitude();
        double clAngle = centerLine.Angle();

        // If the point is on or inside the circle, there are no tangents.
        if (d <= c->radius[0])
                return;

        // Use 'cos(a) = adjacent / hypotenuse' to get the tangent angle
        double a = acos(c->radius[0] / d);

        // Finally, find the points of intersection by using +/- the angle found
        // from the centerline's angle
        Global::intersectPoint[0].x = c->p[0].x + (cos(clAngle + a) * c->radius[0]);
        Global::intersectPoint[0].y = c->p[0].y + (sin(clAngle + a) * c->radius[0]);
        Global::intersectPoint[1].x = c->p[0].x + (cos(clAngle - a) * c->radius[0]);
        Global::intersectPoint[1].y = c->p[0].y + (sin(clAngle - a) * c->radius[0]);
        Global::numIntersectPoints = 2;
}

/*
The extangents can be found by collapsing the circle of smaller radius to a point, and diminishing the larger circle by the radius of the smaller, then treating it like the point-circle case (you have to translate the tangent back out by the length of the radius of the smaller circle once found).

Not sure what the analogous method for finding the intangents are...  :-/
Looks like the intangent can be found by augmenting the larger circle by the smaller radius, and doing the center of the smaller as the point-circle case again, only translating the line back by the radius of the smaller.
*/
void Geometry::FindTangents(Object * c1, Object * c2)
{
        // Set up global vars
        Global::numIntersectPoints = Global::numIntersectParams = 0;

        // Find the larger and smaller of the two:
        Object * cLg = c1, * cSm = c2;

        if (c2->radius[0] > c1->radius[0])
                cLg = c2, cSm = c1;

        // Get the distance between the point and the center of the circle, the
        // length, and the angle.
        Vector centerLine(cLg->p[0], cSm->p[0]);
        double d = centerLine.Magnitude();
        double clAngle = centerLine.Angle();

        // If one circle is completely inside the other, there are no tangents.
        if ((d + cSm->radius[0]) <= cLg->radius[0])
                return;

        // Subtract the radius of the smaller from the larger, and use the point-circle method to find the extangent:
        double a = acos((cLg->radius[0] - cSm->radius[0]) / d);

        // Finally, find the points of intersection by using +/- the angle found
        // from the centerline's angle
        Global::intersectPoint[0].x = cLg->p[0].x + (cos(clAngle + a) * cLg->radius[0]);
        Global::intersectPoint[0].y = cLg->p[0].y + (sin(clAngle + a) * cLg->radius[0]);
        Global::intersectPoint[1].x = cSm->p[0].x + (cos(clAngle + a) * cSm->radius[0]);
        Global::intersectPoint[1].y = cSm->p[0].y + (sin(clAngle + a) * cSm->radius[0]);

        Global::intersectPoint[2].x = cLg->p[0].x + (cos(clAngle - a) * cLg->radius[0]);
        Global::intersectPoint[2].y = cLg->p[0].y + (sin(clAngle - a) * cLg->radius[0]);
        Global::intersectPoint[3].x = cSm->p[0].x + (cos(clAngle - a) * cSm->radius[0]);
        Global::intersectPoint[3].y = cSm->p[0].y + (sin(clAngle - a) * cSm->radius[0]);
        Global::numIntersectPoints = 4;

        // If the circles overlap, there are no intangents.
        if (d <= (cLg->radius[0] + cSm->radius[0]))
                return;

        // Add the radius of the smaller from the larger, and use the point-circle method to find the intangent:
        a = acos((cLg->radius[0] + cSm->radius[0]) / d);

        // Finally, find the points of intersection by using +/- the angle found
        // from the centerline's angle
        Global::intersectPoint[4].x = cLg->p[0].x + (cos(clAngle + a) * cLg->radius[0]);
        Global::intersectPoint[4].y = cLg->p[0].y + (sin(clAngle + a) * cLg->radius[0]);
        Global::intersectPoint[5].x = cSm->p[0].x + (cos(clAngle - a) * cSm->radius[0]);
        Global::intersectPoint[5].y = cSm->p[0].y + (sin(clAngle - a) * cSm->radius[0]);

        Global::intersectPoint[6].x = cLg->p[0].x + (cos(clAngle - a) * cLg->radius[0]);
        Global::intersectPoint[6].y = cLg->p[0].y + (sin(clAngle - a) * cLg->radius[0]);
        Global::intersectPoint[7].x = cSm->p[0].x + (cos(clAngle + a) * cSm->radius[0]);
        Global::intersectPoint[7].y = cSm->p[0].y + (sin(clAngle + a) * cSm->radius[0]);
        Global::numIntersectPoints = 8;
}

//
// Parameter 1: point in question
// Parameter 2, 3: points we are comparing to
//
Point Geometry::NearestTo(Point point, Point p1, Point p2)
{
        double l1 = Vector::Magnitude(point, p1);
        double l2 = Vector::Magnitude(point, p2);

        return (l1 < l2 ? p1 : p2);
}

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;
}

//
// Returns the orientation of the points when traversed in order (from p1 to
// p2 to p3).  +1 = CCW, -1 = CW, 0 = colinear
//
int Geometry::Orientation(Point p1, Point p2, Point p3)
{
        double xx = p2.x - p1.x, yy = p2.y - p1.y;
        double aa = p3.x - p1.x, bb = p3.y - p1.y;

        // Mult by -1 if screen coords already transformed
        double zz = (xx * bb) - (yy * aa);

        return (zz < 0 ? -1 : (zz == 0 ? 0 : 1));
}

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;
}

Arc Geometry::FindArcForThreePoints(Point p1, Point p2, Point p3)
{
        int orientation = Orientation(p1, p2, p3);

        // Sanity check: if points are colinear, there is no arc
        if (orientation == 0)
                return Arc();

        Circle c = FindCircleForThreePoints(p1, p2, p3);
        double a1 = Vector::Angle(c.p[0], p1);
        double a2 = Vector::Angle(c.p[0], p2);
        double span = a2 - a1;

        if (orientation < 0 && span > 0)
                span -= TAU;
        else if (orientation > 0 && span < 0)
                span += TAU;

        return Arc(c.p[0], c.radius[0], a1, span);
}

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 = 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);
}

double Geometry::Pack(Arc * a)
{
        Point p1 = a->p[0] + (Vector(cos(a->angle[0]), sin(a->angle[0])) * a->radius[0]);
        Point p2 = a->p[0] + (Vector(cos(a->angle[0] + a->angle[1]), sin(a->angle[0] + a->angle[1])) * a->radius[0]);
        double endpointLen = Vector::Magnitude(p1, p2) / 2.0;

        // Bump height can be found with h = r ± sqr(r² - a²) where r is the
        // radius, and a is the 1/2 length of the endpoint's line segment.  The
        // plus/minus term is positive if the arc span is less than 1/2 TAU,
        // negative if the arc span is greater than 1/2 TAU.
        double discriminant = sqrt((a->radius[0] * a->radius[0]) - (endpointLen * endpointLen));
        double bumpLen = a->radius[0] + (discriminant * (fabs(a->angle[1]) < HALF_TAU ? 1.0 : -1.0));

        return (bumpLen / endpointLen) * (a->angle[1] > 0 ? -1.0 : 1.0);
}