r5613 - in developers/werner: . cncmap cncmap/millp cncmap/rect
werner at docs.openmoko.org
werner at docs.openmoko.org
Tue Sep 8 00:27:58 CEST 2009
Author: werner
Date: 2009-09-08 00:27:57 +0200 (Tue, 08 Sep 2009)
New Revision: 5613
Added:
developers/werner/cncmap/
developers/werner/cncmap/README
developers/werner/cncmap/millp/
developers/werner/cncmap/millp/Makefile
developers/werner/cncmap/millp/millp.c
developers/werner/cncmap/millp/serial.c
developers/werner/cncmap/millp/serial.h
developers/werner/cncmap/millp/usb.c
developers/werner/cncmap/millp/usb.h
developers/werner/cncmap/rect/
developers/werner/cncmap/rect/Makefile
developers/werner/cncmap/rect/lr.c
developers/werner/cncmap/rect/lr.h
developers/werner/cncmap/rect/rect.c
Log:
CNC tools for prototype development, first batch of them.
Added: developers/werner/cncmap/README
===================================================================
--- developers/werner/cncmap/README (rev 0)
+++ developers/werner/cncmap/README 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,7 @@
+The "cncmap" package consists of the following tools:
+
+millp: interactively control a Roland MDX-15/20 mill, use a "millp" contact
+sensor for precise positioning, print the current coordinates.
+
+rect: find a rectangle defined by three corner points and print its corners
+in a normalized form.
Added: developers/werner/cncmap/millp/Makefile
===================================================================
--- developers/werner/cncmap/millp/Makefile (rev 0)
+++ developers/werner/cncmap/millp/Makefile 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,25 @@
+#
+# millp/Makefile - Build the mill control utility
+#
+# 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=millp
+
+OBJS=millp.o usb.o serial.o
+
+CFLAGS = -Wall -Wshadow
+LDFLAGS = -lusb
+
+$(MAIN): $(OBJS)
+# $(CC) $(LDFLAGS) -o $(MAIN) $(OBJS)
+
+clean:
+ rm -f $(OBJS)
Added: developers/werner/cncmap/millp/millp.c
===================================================================
--- developers/werner/cncmap/millp/millp.c (rev 0)
+++ developers/werner/cncmap/millp/millp.c 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,368 @@
+/*
+ * millp.c - Manual mill control with contact sensing
+ *
+ * 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 <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+
+#include "serial.h"
+#include "usb.h"
+
+
+#define KEY_HOLD_MS 200 /* hold before continuous */
+#define KEY_STEP_MS 50 /* step when continuous */
+
+#define SEEK_STEP_MS 50 /* optimize coords for better res */
+
+#define MIN_X 0 /* 0 cm */
+#define MIN_Y 0 /* 0 cm */
+#define MIN_Z -60 /* 6 cm */
+
+#define MAX_X 150 /* 15 cm */
+#define MAX_Y 100 /* 10 cm */
+#define MAX_Z 0 /* 0 cm */
+
+
+static struct timeval t0; /* base time */
+static unsigned long ms; /* milliseconds since t0 */
+
+static char key = 0; /* current key, 0 if released */
+static unsigned long key_ms; /* milliseconds since last key change */
+static unsigned long key_t0_ms; /* time when the key was pressed/released */
+
+static int probe; /* probe results */
+
+static char dir; /* key of direction in which we seek */
+static unsigned long seek_last_ms;
+static int seek_coarse;
+
+
+static enum {
+ MODE_MANUAL,
+ MODE_SELECT,
+ MODE_SEEK,
+} mode = MODE_MANUAL;
+
+
+#define units(mm) ((mm)*40.0)
+
+
+static double x = 0, y = 0, z = 0;
+static int spindle = 0;
+
+
+static void move(void)
+{
+ serial_printf("!PZ%.1f,0;PD%.1f,%.1f\n", units(z), units(x), units(y));
+}
+
+
+static int rmove_3d(int dx, int dy, int dz, double s)
+{
+ int stop = 0;
+
+ x += dx*s;
+ y += dy*s;
+ z += dz*s;
+
+ if (x < MIN_X) {
+ x = MIN_X;
+ stop = 1;
+ }
+ if (y < MIN_Y) {
+ y = MIN_Y;
+ stop = 1;
+ }
+ if (z < MIN_Z) {
+ z = MIN_Z;
+ stop = 1;
+ }
+
+ if (x > MAX_X) {
+ x = MAX_X;
+ stop = 1;
+ }
+ if (y > MAX_Y) {
+ y = MAX_Y;
+ stop = 1;
+ }
+ if (z > MAX_Z) {
+ z = MAX_Z;
+ stop = 1;
+ }
+
+ move();
+
+ return stop;
+}
+
+
+static void update_time(void)
+{
+ struct timeval now;
+
+ (void) gettimeofday(&now, NULL);
+ now.tv_sec -= t0.tv_sec;
+ now.tv_usec -= t0.tv_usec;
+ ms = now.tv_sec*1000+now.tv_usec/1000;
+}
+
+
+static void do_cycle(int leds)
+{
+ int ch;
+
+ ch = cycle(leds, &probe);
+ if (ch != key) {
+ key = ch;
+ key_t0_ms = ms;
+ }
+ key_ms = ms-key_t0_ms;
+}
+
+
+static void reset_mode(void)
+{
+ mode = MODE_MANUAL;
+// key = 0;
+// key_ms = 0;
+}
+
+
+static int dir_key(char ch, int *dx, int *dy, int *dz)
+{
+ if (dx)
+ *dx = 0;
+ if (dy)
+ *dy = 0;
+ if (dz)
+ *dz = 0;
+
+ switch (ch) {
+ case 4:
+ if (dx)
+ *dx = -1;
+ return 1;
+ case 6:
+ if (dx)
+ *dx = 1;
+ return 1;
+ case 2:
+ if (dy)
+ *dy = -1;
+ return 1;
+ case 8:
+ if (dy)
+ *dy = 1;
+ return 1;
+ case 3:
+ if (dz)
+ *dz = -1;
+ return 1;
+ case 9:
+ if (dz)
+ *dz = 1;
+ return 1;
+ default:
+ return 0;
+ }
+}
+
+
+static int mode_manual(void)
+{
+ static int once = 1;
+ int dx, dy, dz;
+
+ if (dir_key(key, &dx, &dy, &dz)) {
+ if (key_ms < KEY_HOLD_MS) {
+ if (once)
+ rmove_3d(dx, dy, dz, 0.1);
+ } else {
+ if (key_ms > KEY_HOLD_MS+KEY_STEP_MS) {
+ key_t0_ms += KEY_STEP_MS;
+ /* assume 10 mm/s, 50 ms */
+ rmove_3d(dx, dy, dz, 0.5);
+ }
+ }
+ once = 0;
+ return 0;
+ }
+ once = 1;
+
+ switch (key) {
+ case 5:
+ if (key_ms)
+ break;
+ spindle = !spindle;
+ serial_printf("!MC%d\n", spindle);
+ move();
+ break;
+ case 11:
+ mode = MODE_SELECT;
+ dir = 0;
+ break;
+ case 12:
+ return 1;
+ }
+ return 0;
+}
+
+
+static int mode_select(void)
+{
+ switch (key) {
+ case 0:
+ if (!dir)
+ break;
+ mode = MODE_SEEK;
+ seek_last_ms = ms;
+ seek_coarse = 1;
+ break;
+ case 11:
+ break;
+ case 1:
+ case 5:
+ case 7:
+ case 10:
+ case 12:
+ reset_mode();
+ break;
+ default:
+ if (dir_key(key, NULL, NULL, NULL))
+ dir = key;
+ break;
+ }
+ return 0;
+}
+
+
+static int mode_seek(int contact)
+{
+ int dx, dy, dz;
+
+ if (key) {
+ reset_mode();
+ return 0;
+ }
+
+ if (ms < seek_last_ms+SEEK_STEP_MS)
+ return 0;
+
+ seek_last_ms = ms;
+
+ (void) dir_key(dir, &dx, &dy, &dz);
+
+ if (seek_coarse) {
+ if (contact)
+ seek_coarse = 0;
+ else {
+ /* assume 2 mm/s, 50 ms */
+ if (rmove_3d(dx, dy, dz, 0.05))
+ mode = MODE_MANUAL;
+ }
+ } else {
+ if (contact) {
+ dx = -dx;
+ dy = -dy;
+ dz = -dz;
+ if (rmove_3d(dx, dy, dz, 0.01))
+ mode = MODE_MANUAL;
+ } else {
+ mode = MODE_MANUAL;
+ rmove_3d(dx, dy, dz, 0.01);
+ }
+ }
+ return 0;
+}
+
+
+int main(int argc, const char **argv)
+{
+ int show_once = 1;
+ int leds = 0;
+ int contact;
+
+ serial_open("/dev/ttyS0");
+ open_usb();
+ serial_printf("\nIN;!MC0\n");
+ move();
+ (void) gettimeofday(&t0, NULL);
+ while (1) {
+ int hz;
+
+ switch (mode) {
+ case MODE_MANUAL:
+ hz = 0;
+ break;
+ case MODE_SELECT:
+ hz = 1;
+ break;
+ case MODE_SEEK:
+ hz = 10;
+ break;
+ }
+ if (!hz || ((ms/(500/hz)) & 1))
+ leds |= LED_STATE;
+
+ update_time();
+ do_cycle(leds);
+
+ leds = 0;
+ contact = 0;
+ if (!(probe & PROBE_NORMAL)) {
+ leds = LED_CONTACT;
+ contact = 1;
+ if (key != 10)
+ key_ms = 0;
+ } else {
+ if (probe & PROBE_TEST_H)
+ leds |= LED_FAULT_H;
+ if (probe & PROBE_TEST_L)
+ leds |= LED_FAULT_L;
+ if (leds) {
+ key = 0;
+ reset_mode();
+ }
+ }
+
+ if (key == 10 && key_ms > 50 && show_once) {
+ do_cycle(leds & ~LED_STATE);
+ usleep(100000);
+ do_cycle(leds);
+ printf("%.2f %.2f %.2f\n", x, y, z);
+ show_once = 0;
+ }
+ if (!key)
+ show_once = 1;
+
+ switch (mode) {
+ case MODE_MANUAL:
+ if (mode_manual())
+ goto out;
+ break;
+ case MODE_SELECT:
+ if (mode_select())
+ goto out;
+ break;
+ case MODE_SEEK:
+ if (mode_seek(contact))
+ goto out;
+ break;
+ }
+ }
+out:
+ serial_close();
+ do_cycle(0);
+ return 0;
+}
Added: developers/werner/cncmap/millp/serial.c
===================================================================
--- developers/werner/cncmap/millp/serial.c (rev 0)
+++ developers/werner/cncmap/millp/serial.c 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,100 @@
+/*
+ * serial.c - Send data to a Modela MDX-15/20
+ *
+ * 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 <stdarg.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <sys/types.h>
+
+#include "serial.h"
+
+
+static int tty;
+static struct termios old;
+
+
+static void reset_tty(void)
+{
+ if (tty >= 0)
+ if (tcsetattr(tty, TCSADRAIN, &old) < 0)
+ perror("tcsetattr");
+}
+
+
+void serial_open(const char *name)
+{
+ struct termios new;
+
+
+ tty = open(name, O_WRONLY | O_NOCTTY);
+ if (tty < 0) {
+ perror(name);
+ exit(1);
+ }
+
+ if (tcgetattr(tty, &old) < 0) {
+ perror("tcgetattr");
+ exit(1);
+ }
+ atexit(reset_tty);
+
+ new = old;
+ cfmakeraw(&new);
+ cfsetospeed(&new, B9600);
+ new.c_iflag |= IXON;
+ new.c_cflag |= CLOCAL | CRTSCTS;
+ new.c_lflag &= ~ECHO;
+ if (tcsetattr(tty, TCSAFLUSH, &new) < 0) {
+ perror("tcsetattr");
+ exit(1);
+ }
+}
+
+
+void serial_close(void)
+{
+ reset_tty();
+ (void) close(tty);
+ tty = -1;
+}
+
+
+void serial_printf(const char *fmt, ...)
+{
+ char buf[10000]; /* more than enough :) */
+ va_list ap;
+ int len;
+ ssize_t wrote;
+
+ va_start(ap, fmt);
+ len = vsnprintf(buf, sizeof(buf), fmt, ap);
+ va_end(ap);
+ if (len >= sizeof(buf)-1) {
+ fprintf(stderr, "buffer too small\n");
+ exit(1);
+ }
+ wrote = write(tty, buf, strlen(buf));
+ if (wrote < 0) {
+ perror("write");
+ exit(1);
+ }
+ if (wrote != len) {
+ fprintf(stderr, "short write: %d < %d\n",
+ (int) wrote, (int) len);
+ exit(1);
+ }
+}
Added: developers/werner/cncmap/millp/serial.h
===================================================================
--- developers/werner/cncmap/millp/serial.h (rev 0)
+++ developers/werner/cncmap/millp/serial.h 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,21 @@
+/*
+ * serial.h - Send data to a Modela MDX-15/20
+ *
+ * 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 SERIAL_H
+#define SERIAL_H
+
+void serial_open(const char *name);
+void serial_close(void);
+void serial_printf(const char *fmt, ...);
+
+#endif /* !SERIAL_H */
Added: developers/werner/cncmap/millp/usb.c
===================================================================
--- developers/werner/cncmap/millp/usb.c (rev 0)
+++ developers/werner/cncmap/millp/usb.c 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,72 @@
+/*
+ * usb.c - Communicate to the millp control unit
+ *
+ * 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 <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <usb.h>
+
+#include "usb.h"
+
+
+#define TO_DEV 0x40
+#define FROM_DEV 0xc0
+
+
+static usb_dev_handle *dev_handle;
+
+
+void open_usb(void)
+{
+ const struct usb_bus *bus;
+ struct usb_device *dev;
+
+ usb_init();
+ usb_find_busses();
+ usb_find_devices();
+
+ for (bus = usb_get_busses(); bus; bus = bus->next)
+ for (dev = bus->devices; dev; dev = dev->next) {
+ if (dev->descriptor.idVendor != VENDOR_ID)
+ continue;
+ if (dev->descriptor.idProduct != PRODUCT_ID)
+ continue;
+ dev_handle = usb_open(dev);
+ if (dev_handle)
+ return;
+ fprintf(stderr, "can't open device\n");
+ exit(1);
+ }
+ fprintf(stderr, "no device found\n");
+ exit(1);
+}
+
+
+char cycle(int leds, int *probe)
+{
+ uint8_t data[2];
+ int res;
+
+ res = usb_control_msg(dev_handle, FROM_DEV, MILLP_CYCLE, leds, 0,
+ (void *) data, 2, 1000);
+ if (res < 0) {
+ fprintf(stderr, "MILLP_CYCLE: %d\n", res);
+ exit(1);
+ }
+ if (res != 2) {
+ fprintf(stderr, "MILLP_CYCLE: expected 2 bytes, got %d\n", res);
+ exit(1);
+ }
+
+ *probe = data[1];
+ return *data;
+}
Added: developers/werner/cncmap/millp/usb.h
===================================================================
--- developers/werner/cncmap/millp/usb.h (rev 0)
+++ developers/werner/cncmap/millp/usb.h 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,33 @@
+/*
+ * usb.h - Communicate to the millp control unit
+ *
+ * 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 USB_H
+#define USB_H
+
+#define VENDOR_ID 0x1d50 /* borrow from IDBG */
+#define PRODUCT_ID 0x1db6 /* borrow from IDBG */
+
+#define MILLP_CYCLE 0x10 /* we don't need the other commands */
+
+#define LED_FAULT_H 1
+#define LED_FAULT_L 2
+#define LED_CONTACT 4
+#define LED_STATE 8
+
+#define PROBE_NORMAL 1
+#define PROBE_TEST_L 2
+#define PROBE_TEST_H 4
+
+void open_usb(void);
+char cycle(int leds, int *probe);
+
+#endif /* !USB_H */
Added: developers/werner/cncmap/rect/Makefile
===================================================================
--- developers/werner/cncmap/rect/Makefile (rev 0)
+++ developers/werner/cncmap/rect/Makefile 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,25 @@
+#
+# millp/Makefile - Build the mill control utility
+#
+# 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=rect
+
+OBJS=rect.o lr.o
+
+CFLAGS = -Wall -Wshadow
+LDFLAGS = -lm
+
+$(MAIN): $(OBJS)
+# $(CC) $(LDFLAGS) -o $(MAIN) $(OBJS)
+
+clean:
+ rm -f $(OBJS)
Added: developers/werner/cncmap/rect/lr.c
===================================================================
--- developers/werner/cncmap/rect/lr.c (rev 0)
+++ developers/werner/cncmap/rect/lr.c 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,100 @@
+/*
+ * lr.c - Least square approximation of plane
+ *
+ * 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.
+ */
+
+/*
+ * http://en.wikipedia.org/wiki/Linear_least_squares
+ * http://en.wikipedia.org/wiki/Linear_regression#Example
+ * http://en.wikipedia.org/wiki/Cramer's_rule
+ * http://en.wikipedia.org/wiki/Determinant
+ */
+
+
+#include "lr.h"
+
+
+#define MAX_N 1000
+
+
+static double x[MAX_N][3], y[MAX_N];
+static int n = 0;
+
+
+static double det3(double m[3][3])
+{
+ double s = 0;
+ double p;
+ int i, j;
+
+ for (i = 0; i != 3; i++) {
+ p = 1;
+ for (j = 0; j != 3; j++)
+ p *= m[j][(i+j) % 3];
+ s += p;
+ p = 1;
+ for (j = 0; j != 3; j++)
+ p *= m[2-j][(i+j) % 3];
+ s -= p;
+ }
+ return s;
+}
+
+
+static void cramer3(double r[3], double m[3][3], double v[3])
+{
+ double t[3][3];
+ double d;
+ int i, j, k;
+
+ d = det3(m);
+ for (i = 0; i != 3; i++) {
+ for (j = 0; j != 3; j++)
+ for (k = 0; k != 3; k++)
+ t[j][k] = m[j][k];
+ for (j = 0; j != 3; j++)
+ t[j][i] = v[j];
+ r[i] = det3(t)/d;
+ }
+}
+
+
+void lr_point(double px, double py, double pz)
+{
+ x[n][0] = 1;
+ x[n][1] = px;
+ x[n][2] = py;
+ y[n] = pz;
+ n++;
+}
+
+
+void lr_calc(double *z0, double *zx, double *zy)
+{
+ double m[3][3], v[3];
+ double r[3];
+ int i, j, k;
+
+ for (i = 0; i != 3; i++)
+ for (j = 0; j != 3; j++) {
+ m[i][j] = 0;
+ for (k = 0; k != n; k++)
+ m[i][j] += x[k][i]*x[k][j];
+ }
+ for (i = 0; i != 3; i++) {
+ v[i] = 0;
+ for (k = 0; k != n; k++)
+ v[i] += x[k][i]*y[k];
+ }
+ cramer3(r, m, v);
+ *z0 = r[0];
+ *zx = r[1];
+ *zy = r[2];
+}
Added: developers/werner/cncmap/rect/lr.h
===================================================================
--- developers/werner/cncmap/rect/lr.h (rev 0)
+++ developers/werner/cncmap/rect/lr.h 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,20 @@
+/*
+ * lr.h - Least square approximation of plane
+ *
+ * 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 LR_H
+#define LR_H
+
+void lr_point(double x, double y, double z);
+void lr_calc(double *z0, double *zx, double *zy);
+
+#endif /* !LR_H */
Added: developers/werner/cncmap/rect/rect.c
===================================================================
--- developers/werner/cncmap/rect/rect.c (rev 0)
+++ developers/werner/cncmap/rect/rect.c 2009-09-07 22:27:57 UTC (rev 5613)
@@ -0,0 +1,149 @@
+/*
+ * rect.c - Find the rectangle described by three corner points
+ *
+ * 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 "lr.h"
+
+
+#define FUZZ 10 /* rectangularity tolerance, in degrees */
+
+
+static double p[9];
+static int o, a, b; /* offset into "p" of origin, side A, side B */
+static double ox, oy, oz, ax, ay, az, bx, by, bz;
+static double a_len, b_len;
+static double rot; /* angle of A with respect to the x axis */
+static double angle; /* angle from A to B in a counter-clockwise direction */
+static double z0, zx, zy;
+
+
+static void usage(const char *name)
+{
+ fprintf(stderr, "usage: %s x0 y0 z0 ax ay az bx by bz\n", name);
+ exit(1);
+}
+
+
+static void calculate(void)
+{
+ ox = p[o];
+ oy = p[o+1];
+ oz = p[o+2];
+ ax = p[a]-ox;
+ ay = p[a+1]-oy;
+ az = p[a+2]-oz;
+ bx = p[b]-ox;
+ by = p[b+1]-oy;
+ bz = p[b+2]-oz;
+
+ a_len = hypot(ax, ay);
+ rot = atan2(ay, ax);
+ b_len = hypot(bx, by);
+ angle = atan2(by, bx)-rot;
+
+ if (angle < 0)
+ angle += M_PI*2;
+}
+
+
+static double z(double x, double y)
+{
+ return z0+x*zx+y*zy;
+}
+
+
+int main(int argc, char **argv)
+{
+ int i;
+ char *end;
+ double best_angle;
+ int best_o, best_a, best_b;
+ double diff;
+
+ if (argc != 10)
+ usage(*argv);
+ for (i = 0; i != 9; i++) {
+ p[i] = strtod(argv[i+1], &end);
+ if (*end)
+ usage(*argv);
+ }
+
+ for (i = 0; i != 9; i += 3)
+ lr_point(p[i], p[i+1], p[i+2]);
+ lr_calc(&z0, &zx, &zy);
+
+ best_angle = 0;
+ for (o = 0; o != 9; o += 3) {
+ for (a = 0; a != 9; a += 3) {
+ if (a == o)
+ continue;
+ for (b = 0; b != 9; b += 3) {
+ if (b == o || b == a)
+ continue;
+ calculate();
+fprintf(stderr, "%d %d %d -> %f\n", o, a, b, angle/M_PI*180);
+ diff = fabs(angle-M_PI/2);
+ if (diff > fabs(best_angle-M_PI/2))
+ continue;
+ if (diff > FUZZ*M_PI/180)
+ continue;
+ best_angle = angle;
+ best_o = o;
+ best_a = a;
+ best_b = b;
+ }
+ }
+ }
+
+ if (!best_angle) {
+ fprintf(stderr, "no suitable fit found\n");
+ return 1;
+ }
+
+ o = best_o;
+ a = best_a;
+ b = best_b;
+
+ calculate();
+
+ fprintf(stderr, "A = %f, rot %f\n", a_len, rot/M_PI*180);
+ fprintf(stderr, "B = %f, angle %f\n", b_len, angle/M_PI*180);
+ fprintf(stderr, "phi: x %f (%f%%), y %f (%f%%)\n",
+ atan(zx)/M_PI*180, zx*100, atan(zy)/M_PI*180, zy*100);
+
+ /*
+ * Use the angle of the longer side, since it's probably more accurate.
+ */
+ if (a_len > b_len) {
+ angle = rot+M_PI/2;
+ bx = b_len*cos(angle);
+ by = b_len*sin(angle);
+ } else {
+ angle = rot+angle-M_PI/2;
+ ax = a_len*cos(angle);
+ ay = a_len*sin(angle);
+ }
+
+ ax += ox;
+ ay += oy;
+ bx += ox;
+ by += oy;
+
+ printf("%.2f %.2f %.2f\n", ox, oy, z(ox, oy));
+ printf("%.2f %.2f %.2f\n", ax, ay, z(ax, ay));
+ printf("%.2f %.2f %.2f\n", bx, by, z(bx, by));
+
+ return 0;
+}
More information about the commitlog
mailing list
