r5701 - in developers/werner/cncmap: . rect zmap
werner at docs.openmoko.org
werner at docs.openmoko.org
Thu Oct 22 15:23:26 CEST 2009
Author: werner
Date: 2009-10-22 15:23:25 +0200 (Thu, 22 Oct 2009)
New Revision: 5701
Added:
developers/werner/cncmap/zmap/
developers/werner/cncmap/zmap/Makefile
developers/werner/cncmap/zmap/try.c
developers/werner/cncmap/zmap/zline.c
developers/werner/cncmap/zmap/zline.h
developers/werner/cncmap/zmap/zmap.c
developers/werner/cncmap/zmap/zmap.h
Modified:
developers/werner/cncmap/rect/Makefile
Log:
Project a line on the Z map. Work in progress.
Modified: developers/werner/cncmap/rect/Makefile
===================================================================
--- developers/werner/cncmap/rect/Makefile 2009-10-21 20:11:17 UTC (rev 5700)
+++ developers/werner/cncmap/rect/Makefile 2009-10-22 13:23:25 UTC (rev 5701)
@@ -1,5 +1,5 @@
#
-# millp/Makefile - Build the mill control utility
+# rect/Makefile - Build the rectangle constructor
#
# Written 2008, 2009 by Werner Almesberger
# Copyright 2008, 2009 Werner Almesberger
Added: developers/werner/cncmap/zmap/Makefile
===================================================================
--- developers/werner/cncmap/zmap/Makefile (rev 0)
+++ developers/werner/cncmap/zmap/Makefile 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,25 @@
+#
+# zmap/Makefile - Build the Z axis mapper
+#
+# Written 2008, 2009 by Werner Almesberger
+# Copyright 2008, 2009 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=zline
+
+OBJS=try.o zline.o zmap.o
+
+CFLAGS = -Wall -Wshadow
+LDFLAGS = -lm
+
+$(MAIN): $(OBJS)
+# $(CC) $(LDFLAGS) -o $(MAIN) $(OBJS)
+
+clean:
+ rm -f $(OBJS)
Added: developers/werner/cncmap/zmap/try.c
===================================================================
--- developers/werner/cncmap/zmap/try.c (rev 0)
+++ developers/werner/cncmap/zmap/try.c 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,37 @@
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "zline.h"
+
+
+static void point (void *user, double x, double y, double z)
+{
+ printf("%f %f %f\n", x, y, z);
+}
+
+
+static void usage(const char *name)
+{
+ fprintf(stderr, "usage: %s zmap xa ya xb yb\n", name);
+ exit(1);
+}
+
+
+int main(int argc, char **argv)
+{
+ char *end;
+ double p[4];
+ int i;
+
+ if (argc != 6)
+ usage(*argv);
+ zline_init(argv[1]);
+ for (i = 0; i != 4; i++) {
+ p[i] = strtod(argv[i+2], &end);
+ if (*end)
+ usage(*argv);
+ }
+ zline(p[0], p[1], p[2], p[3], point, NULL);
+ zline_end();
+ return 0;
+}
Added: developers/werner/cncmap/zmap/zline.c
===================================================================
--- developers/werner/cncmap/zmap/zline.c (rev 0)
+++ developers/werner/cncmap/zmap/zline.c 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,156 @@
+/*
+ * zline.c - Iterate a line on the Z map
+ *
+ * Written 2009 by Werner Almesberger
+ * Copyright 2009 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 <assert.h>
+
+#include "zmap.h"
+#include "zline.h"
+
+
+static struct xyz *map; /* Z map */
+static struct proj *proj;
+static int map_n;
+
+
+void zline_init(const char *name)
+{
+ map = zmap_read(name, &map_n);
+}
+
+
+/*
+ * Find when another point than "ref" becomes closest to the line as we move
+ * along it (increasing t).
+ */
+
+static int next_closest(double xa, double ya, double xb, double yb,
+ int ref, int start, int n, double *t)
+{
+ int i;
+ double x, y, dx, dy, pos;
+ double best_t = 1;
+ int best_point = -1;
+
+ for (i = start; i != n; i++) {
+ /*
+ * Eliminate points that can't possibly be any closer. This
+ * also avoids the delta_t == 0 case, where we'd cross the
+ * line somewhere in infinity.
+ */
+ if (proj[i].t <= proj[ref].t)
+ continue;
+
+ /*
+ * Assume a triangle (0, 0) - (t, 0) - (t, d). A normal on
+ * (0, 0) - (t, d) projected from (t, d) crosses the x axis at
+ * (d^2/t, 0).
+ *
+ * The formula below is for the same calculation, but with the
+ * triangle at (t0, d0) - (t1, d0) - (t1, d1), where (t0, d0)
+ * is point "ref" and (t1, d1) is the middle between points "i"
+ * and "ref".
+ */
+ x = (proj[i].t+proj[ref].t)/2;
+ y = (proj[i].d+proj[ref].d)/2;
+ dx = proj[i].t-proj[ref].t;
+ dy = proj[i].d-proj[ref].d;
+ pos = x+dy*y/dx;
+
+ /* don't go back */
+ if (pos <= *t)
+ continue;
+
+ if (pos < best_t) {
+ best_t = pos;
+ best_point = i;
+ }
+ }
+ *t = best_t;
+ return best_point;
+}
+
+
+static void swap(int a, int b)
+{
+ struct xyz tmp_xyz;
+ struct proj tmp_proj;
+
+ tmp_xyz = map[a];
+ map[a] = map[b];
+ map[b] = tmp_xyz;
+
+ tmp_proj = proj[a];
+ proj[a] = proj[b];
+ proj[b] = tmp_proj;
+}
+
+
+/*
+ * We iterate along a line as follows: for the starting point, we find the
+ * three Z map points closest to it, then use zmap_point.
+ *
+ * For the next point, we determine which of the currently closest points gets
+ * replaced by a new closer point next as we move towards the end of the line.
+ * We then reorder and shorten the list such that the old point drops off it
+ * and the new one takes its position.
+ */
+
+void zline(double xa, double ya, double xb, double yb,
+ void (*point)(void *user, double x, double y, double z), void *user)
+{
+ double t;
+ int n;
+ double best_t, tmp_t;
+ int best_next, best_old;
+ int i, tmp_i;
+ double x, y;
+
+ assert(map_n >= 3);
+ zmap_sort(map, map_n, xa, ya);
+ proj = zmap_project(map, map_n, xa, ya, xb, yb);
+ point(user, xa, ya, zmap_point(map[0], map[1], map[2], xa, ya));
+
+ t = 0;
+ n = map_n;
+ while (n >= 3) {
+ best_t = 1;
+ best_next = best_old = -1;
+ for (i = 0; i != 3; i++) {
+ tmp_i = next_closest(xa, ya, xb, yb, i, 3, n, &tmp_t);
+ if (tmp_t < best_t) {
+ best_t = tmp_t;
+ best_next = tmp_i;
+ best_old = i;
+ }
+ }
+ if (best_next == -1)
+ break;
+ swap(best_old, n-1);
+ swap(best_old, best_next);
+ n--;
+ t = best_t;
+ x = xa+(xb-xa)*t;
+ y = ya+(yb-ya)*t;
+ point(user, x, y, zmap_point(map[0], map[1], map[2], x, y));
+ }
+
+ point(user, xb, yb, zmap_point(map[0], map[1], map[2], xb, yb));
+ free(proj);
+}
+
+
+void zline_end(void)
+{
+ free(map);
+}
Added: developers/werner/cncmap/zmap/zline.h
===================================================================
--- developers/werner/cncmap/zmap/zline.h (rev 0)
+++ developers/werner/cncmap/zmap/zline.h 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,22 @@
+/*
+ * zline.h - Iterate a line on the Z map
+ *
+ * Written 2009 by Werner Almesberger
+ * Copyright 2009 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.
+ */
+
+
+#ifndef ZLINE_H
+#define ZLINE_H
+
+void zline_init(const char *name);
+void zline(double xa, double ya, double xb, double yb,
+ void (*point)(void *user, double x, double y, double z), void *user);
+void zline_end(void);
+
+#endif /* ZLINE_H */
Added: developers/werner/cncmap/zmap/zmap.c
===================================================================
--- developers/werner/cncmap/zmap/zmap.c (rev 0)
+++ developers/werner/cncmap/zmap/zmap.c 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,135 @@
+/*
+ * zmap.c - Basic operations on a Z map
+ *
+ * Written 2009 by Werner Almesberger
+ * Copyright 2009 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>
+
+#include "zmap.h"
+
+
+struct xyz *zmap_read(const char *name, int *n)
+{
+ FILE *file;
+ struct xyz tmp;
+ struct xyz *map = NULL;
+
+ file = fopen(name, "r");
+ if (!file) {
+ perror(name);
+ exit(1);
+ }
+ *n = 0;
+ while (fscanf(file, "%lf %lf %lf", &tmp.x, &tmp.y, &tmp.z) == 3) {
+ map = realloc(map, sizeof(struct xyz)*(*n+1));
+ if (!map) {
+ perror("realloc");
+ exit(1);
+ }
+ map[*n] = tmp;
+ (*n)++;
+ }
+ fclose(file);
+ return map;
+}
+
+
+static double g_x, g_y;
+
+
+static int comp(const void *_a, const void *_b)
+{
+ const struct xyz *a = _a;
+ const struct xyz *b = _a;
+ double d_a, d_b;
+
+ d_a = hypot(a->x-g_x, a->y-g_y);
+ d_b = hypot(b->x-g_x, b->y-g_y);
+ return d_a < d_b ? -1 : d_a != d_b;
+}
+
+
+void zmap_sort(struct xyz *map, int n, double x, double y)
+{
+ g_x = x;
+ g_y = y;
+ qsort(map, n, sizeof(struct xyz), comp);
+}
+
+
+struct proj *zmap_project(const struct xyz *map, int n,
+ double xa, double ya, double xb, double yb)
+{
+ struct proj *p;
+ double b, t;
+ int i;
+
+ p = malloc(sizeof(struct proj)*n);
+ if (!p) {
+ perror("malloc");
+ exit(1);
+ }
+ xb -= xa;
+ yb -= ya;
+ b = hypot(xb, yb);
+ for (i = 0; i != n; i++) {
+ t = ((map[i].x-xa)*xb+(map[i].y-ya)*yb)/b/b;
+ p[i].t = t;
+ p[i].d = hypot(xa+xb*t-map[i].x, ya+yb*t-map[i].y);
+ }
+ return p;
+}
+
+
+/*
+ * a, b, and c define a plane in 3D space that's not normal to the xy plane.
+ * zmap_point returns the z coordinate of the point above position (x, y).
+ *
+ * We use this to interpolate (and sometimes extrapolate) locations on the z
+ * map. The basic idea is as follows:
+ *
+ * - for each point whose "height" we want to know, we find the three points in
+ * the z map that are closest in the xy plane.
+ *
+ * - with these points, we invoke zmap_point
+ *
+ * Note that this algorithm may amplify measurement errors, particularly when
+ * extrapolating. It would be safer to use the four closest points and run the
+ * least square approximation (../rect/lr.c) on them.
+ */
+
+double zmap_point(struct xyz a, struct xyz b, struct xyz c, double x, double y)
+{
+ double xp, yp;
+ double xu, yu, zu, u;
+ double xv, yv, zv, v;
+ double s, t;
+
+ xp = x-a.x;
+ yp = y-a.y;
+
+ xu = b.x-a.x;
+ yu = b.y-a.y;
+ zu = b.z-a.z;
+ u = hypot(xu, yu);
+
+ xv = c.x-a.x;
+ yv = c.y-a.y;
+ zv = c.z-a.z;
+ v = hypot(xv, yv);
+
+ s = (xp*xu+yp*yu)/u/u;
+ t = (xp*xv+yp*yv)/v/v;
+
+ return a.z+s*zu+t*zv;
+}
Added: developers/werner/cncmap/zmap/zmap.h
===================================================================
--- developers/werner/cncmap/zmap/zmap.h (rev 0)
+++ developers/werner/cncmap/zmap/zmap.h 2009-10-22 13:23:25 UTC (rev 5701)
@@ -0,0 +1,32 @@
+/*
+ * zmap.h - Basic operations on a Z map
+ *
+ * Written 2009 by Werner Almesberger
+ * Copyright 2009 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.
+ */
+
+
+#ifndef ZMAP_H
+#define ZMAP_H
+
+struct xyz {
+ double x, y, z;
+};
+
+struct proj {
+ double t; /* p = a+b*t */
+ double d; /* distance between map point and line */
+};
+
+struct xyz *zmap_read(const char *name, int *n);
+void zmap_sort(struct xyz *map, int n, double x, double y);
+struct proj *zmap_project(const struct xyz *map, int n,
+ double xa, double ya, double xb, double yb);
+double zmap_point(struct xyz a, struct xyz b, struct xyz c, double x, double y);
+
+#endif /* ZMAP_H */
More information about the commitlog
mailing list
