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