X-Git-Url: http://shamusworld.gotdns.org/cgi-bin/gitweb.cgi?a=blobdiff_plain;f=src%2Fgeometry.cpp;h=f4918560990d8ddcd61709daf54aae49c6b4fadc;hb=10cf4c797bed05831e976068b7504908279dc997;hp=31c5257d8d0aa9f39c5924224f11d40359191d7e;hpb=fce575a51ba1f26418869c20e63b9f388e118ab6;p=architektonas

diff --git a/src/geometry.cpp b/src/geometry.cpp
index 31c5257..f491856 100644
--- a/src/geometry.cpp
+++ b/src/geometry.cpp
@@ -512,3 +512,112 @@ Point Geometry::NearestTo(Point point, Point p1, 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;
+}
+
+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);
+}