r5778 - in developers/werner/cncmap: . align
werner at docs.openmoko.org
werner at docs.openmoko.org
Mon Jan 4 18:09:20 CET 2010
Author: werner
Date: 2010-01-04 18:09:19 +0100 (Mon, 04 Jan 2010)
New Revision: 5778
Added:
developers/werner/cncmap/align/
developers/werner/cncmap/align/Makefile
developers/werner/cncmap/align/align.c
Log:
Added "align" tool to map model coordinates to the workpiece.
Added: developers/werner/cncmap/align/Makefile
===================================================================
--- developers/werner/cncmap/align/Makefile (rev 0)
+++ developers/werner/cncmap/align/Makefile 2010-01-04 17:09:19 UTC (rev 5778)
@@ -0,0 +1,25 @@
+#
+# align/Makefile - Build the model aligner
+#
+# Written 2008-2010 by Werner Almesberger
+# Copyright 2008-2010 Werner Almesberger
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+
+
+MAIN=align
+
+OBJS=align.o
+
+CFLAGS = -g -Wall -Wshadow
+LDFLAGS = -lm
+
+$(MAIN): $(OBJS)
+# $(CC) $(LDFLAGS) -o $(MAIN) $(OBJS)
+
+clean:
+ rm -f $(OBJS)
Added: developers/werner/cncmap/align/align.c
===================================================================
--- developers/werner/cncmap/align/align.c (rev 0)
+++ developers/werner/cncmap/align/align.c 2010-01-04 17:09:19 UTC (rev 5778)
@@ -0,0 +1,224 @@
+/*
+ * align.c - Align the model with the workpiece
+ *
+ * Written 2009, 2010 by Werner Almesberger
+ * Copyright 2009, 2010 Werner Almesberger
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+
+/* ----- reorient ---------------------------------------------------------- */
+
+
+/*
+ * For alignment, we prefer a different normalization than the one "rect" gives
+ * us. We use:
+ * - lower left corner, followed by
+ * - rightmost low corner, followed by
+ * - topmost left corner
+ *
+ * The heuristics below only work if the workpiece is more or less parallel to
+ * the axes and not extremely elongated.
+ */
+
+static int reorient_pick(double xm, double ym, double x, double y)
+{
+ if (x < xm && y < ym)
+ return 0;
+ if (x > xm && y < ym)
+ return 1;
+ if (x < xm && y > ym)
+ return 2;
+ return -1;
+}
+
+
+static int reorient(double q[6], double xm, double ym, double x, double y)
+{
+ int pick;
+
+ pick = reorient_pick(xm, ym, x, y);
+ if (pick < 0)
+ return 0;
+ q[pick*2] = x;
+ q[pick*2+1] = y;
+ return 1 << pick;
+}
+
+
+static void make_matrix(double m[3][2], double angle, double ox, double oy,
+ int ref, double p[9])
+{
+ double xm, ym;
+ double q[6];
+ int r1, r2, r3, r4;
+ double x, y, a;
+
+ xm = (p[2]+p[4])/2;
+ ym = (p[3]+p[5])/2;
+ r1 = reorient(q, xm, ym, p[0], p[1]);
+ r2 = reorient(q, xm, ym, p[2], p[3]);
+ r3 = reorient(q, xm, ym, p[4], p[5]);
+ r4 = reorient(q, xm, ym, p[2]+p[4]-p[0], p[3]+p[5]-p[1]);
+
+ if ((r1 | r2 | r3 | r4) != 7 ||
+ (r1 & r2) || (r1 & r3) || (r1 & r4) ||
+ (r2 & r3) || (r2 & r4) ||
+ (r3 & r4)) {
+ fprintf(stderr, "workpiece orientation is too weird\n");
+ exit(1);
+ }
+
+ x = q[0]+((ref-1) % 3)*(q[2]-q[0])/2+((ref-1)/3)*(q[4]-q[0])/2;
+ y = q[1]+((ref-1) % 3)*(q[3]-q[1])/2+((ref-1)/3)*(q[5]-q[1])/2;
+
+ a = atan2(q[3]-q[1], q[2]-q[0]);
+ a += angle/180.0*M_PI;
+
+ /*
+ * x' = m[0][0]+x*m[1][0]+y*m[2][0]
+ * y' = m[0][1]+x*m[1][1]+y*m[2][1]
+ */
+
+ m[1][0] = cos(a);
+ m[2][0] = -sin(a);
+ m[1][1] = sin(a);
+ m[2][1] = cos(a);
+ m[0][0] = x-ox*m[1][0]-oy*m[2][0];
+ m[0][1] = y-ox*m[1][1]-oy*m[2][1];
+}
+
+
+/*
+ * Returns the lenght of the projection of OP on OA, normalized to the range
+ * 0 if P = O and 1 if P = A.
+ */
+
+static double project(double px, double py, double ox, double oy,
+ double ax, double ay)
+{
+ ax -= ox;
+ ay -= oy;
+ return ((px-ox)*ax+(py-oy)*ay)/(ax*ax+ay*ay);
+}
+
+
+static void process_file(FILE *file, double m[3][2], double p[6])
+{
+ static int warn = 10;
+ int lineno = 0;
+ char buf[1024];
+ double x, y, z;
+ double rx, ry, f1, f2;
+ int n;
+
+ while (fgets(buf, sizeof(buf), file)) {
+ lineno++;
+ n = sscanf(buf, "%lf %lf %lf\n", &x, &y, &z);
+ switch (n) {
+ case -1:
+ printf("\n");
+ continue;
+ case 2:
+ case 3:
+ rx = m[0][0]+x*m[1][0]+y*m[2][0];
+ ry = m[0][1]+x*m[1][1]+y*m[2][1];
+ f1 = project(rx, ry, p[0], p[1], p[2], p[3]);
+ f2 = project(rx, ry, p[0], p[1], p[4], p[5]);
+ if (f1 < 0 || f1 > 1 || f2 < 0 || f2 > 1) {
+ if (warn) {
+ fprintf(stderr,
+ "warning: point %f %f (from %f %f)"
+ " is outside the workpiece\n",
+ rx, ry, x, y);
+ warn--;
+ } else {
+ fprintf(stderr, "(more warnings)\n");
+ }
+ }
+ printf("%f %f", rx, ry);
+ if (n == 2)
+ printf("\n");
+ else
+ printf(" %f\n", z);
+ break;
+ default:
+ fprintf(stderr, "invalid data at line %d\n", lineno);
+ exit(1);
+ }
+ }
+}
+
+
+static void usage(const char *name)
+{
+ fprintf(stderr,
+"usage: %s angle [ox oy] reference x0 y0 ax ay bx by\n\n"
+" angle counter-clockwise rotation to apply to input\n"
+" before aligning, in degrees\n"
+" ox oy origin of model, in mm (default: 0 0) 7 8 9\n"
+" reference reference point for alignment, as shown here: 4 5 6\n"
+" 1 is lower left corner, 5 is center, etc. 1 2 3\n"
+" x0 y0 ax ay bx by\n"
+" normalized rectangle, as provided by \"rect\"\n",
+ name);
+ exit(1);
+}
+
+
+int main(int argc, const char **argv)
+{
+ double angle, ox, oy, p[6];
+ char *end;
+ int ref, i;
+ double m[3][2];
+
+ switch (argc) {
+ case 9:
+ ox = 0;
+ oy = 0;
+ break;
+ case 11:
+ ox = strtod(argv[2], &end);
+ if (*end)
+ usage(*argv);
+ oy = strtod(argv[3], &end);
+ if (*end)
+ usage(*argv);
+ break;
+ default:
+ usage(*argv);
+ }
+
+ angle = strtod(argv[1], &end);
+ if (*end)
+ usage(*argv);
+
+ for (i = 0; i != 6; i++) {
+ p[i] = strtod(argv[argc-6+i], &end);
+ if (*end)
+ usage(*argv);
+ }
+
+ if (!argv[argc-7][0] || argv[argc-7][1])
+ usage(*argv);
+ ref = atoi(argv[argc-7]);
+ if (ref < 1 || ref > 9)
+ usage(*argv);
+
+ make_matrix(m, angle, ox, oy, ref, p);
+ process_file(stdin, m, p);
+
+ return 0;
+}
+
+//*** range check
More information about the commitlog
mailing list
