ptrude/: many major math fixes, especially in stretch_path

ptrude/path.c

@@ -14,6 +14,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <math.h>
+#include <assert.h>
 
 #include "ptrude.h"
 #include "path.h"
@@ -127,14 +128,16 @@ static void adjust_length(struct vertex *from, struct vertex *to, double len)
 	struct vertex *v;
 	double sum, f;
 
+	if (from == to)
+		return;
 	sum = 0;
-	for (v = from; v != to; v = v->next) {
+	for (v = from->next; v != to; v = v->next) {
 		v->len = hypot(v->x-v->next->x, v->y-v->next->y);
 		sum += v->len;
 	}
 
 	f = len/sum;
-	for (v = from; v != to; v = v->next)
+	for (v = from->next; v != to; v = v->next)
 		v->len *= f;
 }
 
@@ -178,6 +181,7 @@ static struct vertex *corner(struct path *path, struct vertex *a,
 	int n;		/* number of connecting segments (0 if none) */
 
 	double f;	/* scale factor; various uses */
+	double fa, fb;	/* scale factors for first and last vertex */
 	double ang;	/* current angle, for iteration */
 	double x, y;	/* current position; for iteration */
 	int i;		/* segment; for iteration */
@@ -226,15 +230,21 @@ static struct vertex *corner(struct path *path, struct vertex *a,
 	 * available length in half, one for the inbound arc, the other for the
 	 * outbound arc.
 	 */
+
+	/*
+	 * @@@ Our error checking is a bit overzealous and doesn't provide
+	 * enough information to debug any problems. Turn errors into warnings
+	 * for now.
+	 */
 	if (aa/2 < s) {
 		fprintf(stderr, "first vector is too short (%g/2 < %g)\n",
 		    aa, s);
-		exit(1);
+//		exit(1);
 	}
 	if (bb/2 < s) {
 		fprintf(stderr, "second vector is too short (%g/2 < %g)\n",
 		    bb, s);
-		exit(1);
+//		exit(1);
 	}
 
 	/*
@@ -279,11 +289,11 @@ static struct vertex *corner(struct path *path, struct vertex *a,
 	 * Step 4: emit the starting point of the arc
 	 */
 
-	f = s/aa;
-	x = b->x-f*ax;
-	y = b->y-f*ay;
+	fa = s/aa;
+	x = b->x-fa*ax;
+	y = b->y-fa*ay;
 	v0 = add_vertex(path, x, y, b->r, b->d, b->tag);
-	a->len = a->len-s;
+	v0->len = a->len*(1-fa);
 
 	/*
 	 * Step 5: determine if we need intermediate points. If yes, how many,
@@ -337,15 +347,15 @@ static struct vertex *corner(struct path *path, struct vertex *a,
 	 * Step 6: emit the finishing point of the arc
 	 */
 
-	f = s/bb;
-	v1 = add_vertex(path, b->x+f*bx, b->y+f*by, 0, 0, NULL);
-	v1->len = b->len-s;
+	fb = s/bb;
+	v1 = add_vertex(path, b->x+fb*bx, b->y+fb*by, 0, 0, NULL);
+	v1->len = b->len*(1-fb);
 
 	/*
 	 * Step 7: adjust the nominal length of the segments
 	 */
 
-	adjust_length(v0, v1, 2*s);
+	adjust_length(v0, v1, a->len*fa+b->len*fb);
 
 
 	return v1;
@@ -386,18 +396,19 @@ struct path *round_path(const struct path *path, double r, double d)
 
 
 static void move_vertex(struct path *path, const struct vertex *v,
-    double nx, double ny, double d)
+    double nx, double ny, double dist, double r)
 {
 	struct vertex *new;
 
 	new = clone_vertex(v);
-	new->x += nx*d;
-	new->y += ny*d;
+	new->x += nx*dist;
+	new->y += ny*dist;
+	new->r = r;
 	append_vertex(path, new);
 }
 
 
-struct path *stretch_path(const struct path *path, double d)
+struct path *stretch_path(const struct path *path, double dist, double r)
 {
 	struct path *new;		/* new path */
 	const struct vertex *v;		/* current vertex (for iteration) */
@@ -413,9 +424,13 @@ struct path *stretch_path(const struct path *path, double d)
 	nx = b->y-a->y;
 	ny = a->x-b->x;
 	f = hypot(nx, ny);
-	move_vertex(new, a, nx/f, ny/f, d);
+	if (a->r)
+		r = a->r;
+	move_vertex(new, a, nx/f, ny/f, dist, r);
 
 	for (v = path->vertices->next; v->next; v = v->next) {
+		double tmp;
+
 		b = v;
 		c = v->next;
 
@@ -425,12 +440,40 @@ struct path *stretch_path(const struct path *path, double d)
 		nx = tx/f;
 		ny = ty/f;
 
+		tmp = f;
+
 		tx = c->y-b->y;
 		ty = b->x-c->x;
 		f = hypot(tx, ty);
 		nx += tx/f;
 		ny += ty/f;
-		move_vertex(new, b, nx/2, ny/2, d);
+		if (b->r)
+			r = b->r;
+
+		f = hypot(nx, ny);
+		nx /= f;
+		ny /= f;
+
+		/*
+		 * We have this far:
+		 * nx, ny = normal on corner, normalized
+		 * tmp = |a|, length of vector "a" (A -> B)
+		 * dist = the distance by which we stretch
+		 *
+		 * As shown in stretch.fig, we the length we need is
+		 * d' = d/cos(90-t)
+		 *
+		 * With
+		 * http://en.wikipedia.org/wiki/Trigonometric_identities#Symmetry
+		 * cos(90-t) = sin t = (n x a)/(|n|*|a|)
+		 *
+		 * Thus
+		 * d' = d/sin(t) - d*(|n|*|a|)/(n x a)
+		 *    = d/sin(t) - d*|a|/(n x a)
+		 */
+		tmp = dist*tmp/(nx*(b->y-a->y)-ny*(b->x-a->x));
+
+		move_vertex(new, b, nx, ny, tmp, r+dist);
 
 		a = v;
 	}
@@ -438,7 +481,9 @@ struct path *stretch_path(const struct path *path, double d)
 	nx = v->y-a->y;
 	ny = a->x-v->x;
 	f = hypot(nx, ny);
-	move_vertex(new, v, nx/f, ny/f, d);
+	if (v->r)
+		r = v->r;
+	move_vertex(new, v, nx/f, ny/f, dist, r);
 
 	return new;
 }
@@ -485,6 +530,7 @@ struct path *load_path(FILE *file)
 		tag = NULL;
 	}
 
+	path_set_length(path);
 	return path;
 }