Introducing cameo, a toolbox of CAM operations.

cameo/path.c

@@ -0,0 +1,217 @@
+/*
+ * path.c - Toolpath operations
+ *
+ * Written 2010 by Werner Almesberger
+ * Copyright 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 <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <assert.h>
+
+#include "util.h"
+#include "path.h"
+
+
+struct path *path_new(double r_tool)
+{
+	struct path *path;
+
+	path = alloc_type(struct path);
+	path->r_tool = r_tool;
+	path->first = path->last = NULL;
+	path->next = NULL;
+	return path;
+}
+
+
+static struct point *clone_point(struct point *p)
+{
+	struct point *n;
+
+	n = alloc_type(struct point);
+	n->x = p->x;
+	n->y = p->y;
+	n->z = p->z;
+	n->next = NULL;
+	return n;
+}
+
+
+static int path_is_closed(const struct path *path)
+{
+	if (path->first == path->last)
+		return 1;
+	return path->first->x == path->last->x &&
+	    path->first->y == path->last->y && path->first->z == path->last->z;
+}
+
+
+static void path_add_point(struct path *path, struct point *p)
+{
+	p->next = NULL;
+	if (path->last)
+		path->last->next = p;
+	else
+		path->first = p;
+	path->last = p;
+}
+
+
+void path_add(struct path *path, double x, double y, double z)
+{
+	struct point *p;
+
+	p = alloc_type(struct point);
+	p->x = x;
+	p->y = y;
+	p->z = z;
+	return path_add_point(path, p);
+}
+
+
+struct path *path_reverse(const struct path *path)
+{
+	struct path *new;
+	const struct point *p;
+	struct point *n;
+
+	new = path_new(path->r_tool);
+	for (p = path->first; p; p = p->next) {
+		n = alloc_type(struct point);
+		n->x = p->x;
+		n->y = p->y;
+		n->z = p->z;
+		n->next = new->first;
+		if (!new->last)
+			new->last = n;
+		new->first = n;
+	}
+	return new;
+}
+
+
+static struct point *offset_point(const struct point *a, const struct point *b,
+    const struct point *c, double off)
+{
+	double ax, ay, bx, by;
+	double aa, bb;
+	double nx, ny;
+	struct point *p;
+
+	ax = b->x-a->x;
+	ay = b->y-a->y;
+	bx = c->x-b->x;
+	by = c->y-b->y;
+
+	aa = hypot(ax, ay);
+	bb = hypot(bx, by);
+	nx = -(ay/aa+by/bb);
+	ny = ax/aa+bx/bb;
+
+	nx *= off;
+	ny *= off;
+
+	p = alloc_type(struct point);
+	p->x = b->x+nx;
+	p->y = b->y+ny;
+	p->z = b->z;
+	p->next = NULL;
+
+	return p;
+}
+
+
+static int left_turn(const struct point *a, const struct point *b,
+    const struct point *c)
+{
+	double ax, ay, bx, by;
+
+	ax = b->x-a->x;
+	ay = b->y-a->y;
+	bx = c->x-b->x;
+	by = c->y-b->y;
+
+	return (ax*by-ay*bx) >= 0;
+}
+
+
+/*
+ * http://www.makecnc.com/bones.jpg
+ */
+
+
+static struct point *dog_point(const struct point *edge,
+    const struct point *tool, double off)
+{
+	double vx, vy, v;
+	struct point *p;
+
+	vx = edge->x-tool->x;
+	vy = edge->y-tool->y;
+	v = hypot(vx, vy);
+
+	vx *= 1-off/v;
+	vy *= 1-off/v;
+
+	p = alloc_type(struct point);
+	p->x = tool->x+vx;
+	p->y = tool->y+vy;
+	p->z = tool->z;
+	p->next = NULL;
+
+	return p;
+}
+
+
+/*
+ * The tool is on the "left" side of the path. E.g., if the path goes from
+ * 6 o'clock to 12 o'clock, the tool would be at 9 o'clock.
+ */
+
+struct path *path_offset(const struct path *path, int notch)
+{
+	struct path *new;
+	const struct point *prev, *p, *next;
+	struct point *n, *n2;
+	int dog;
+
+	assert(path_is_closed(path));
+	new = path_new(path->r_tool);
+	prev = path->first;
+	for (p = path->first->next; p; p = p->next) {
+		next = p->next ? p->next : path->first->next;
+		n = offset_point(prev, p, next, path->r_tool);
+		dog = notch && left_turn(prev, p, next);
+		if (dog)
+			n2 = clone_point(n);
+		path_add_point(new, n);
+		if (dog) {
+			path_add_point(new, dog_point(p, n2, path->r_tool));
+			path_add_point(new, n2);
+		}
+		prev = p;
+	}
+	path_add_point(new, clone_point(new->first));
+	return new;
+}
+
+
+void path_free(struct path *path)
+{
+	struct point *next;
+
+	while (path->first) {
+		next = path->first->next;
+		free(path->first);
+		path->first = next;
+	}
+	free(path);
+}