396 lines
7.1 KiB
C
396 lines
7.1 KiB
C
/*
|
|
* path.c - Toolpath operations
|
|
*
|
|
* Written 2010-2012, 2015 by Werner Almesberger
|
|
* Copyright 2010-2012, 2015 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 "shape.h"
|
|
#include "poly2d.h"
|
|
#include "path.h"
|
|
|
|
|
|
static void free_points(struct point *points)
|
|
{
|
|
struct point *next;
|
|
|
|
while (points) {
|
|
next = points->next;
|
|
free(points);
|
|
points = next;
|
|
}
|
|
}
|
|
|
|
|
|
void path_free(struct path *path)
|
|
{
|
|
free_points(path->first);
|
|
free((void *) path->id);
|
|
free(path);
|
|
}
|
|
|
|
|
|
struct path *path_new(double r_tool, const char *id)
|
|
{
|
|
struct path *path;
|
|
|
|
path = alloc_type(struct path);
|
|
path->r_tool = r_tool;
|
|
path->outside = 0;
|
|
path->notch = 0;
|
|
path->id = id ? stralloc(id) : NULL;
|
|
path->first = path->last = NULL;
|
|
path->next = NULL;
|
|
return path;
|
|
}
|
|
|
|
|
|
struct path *path_from(const struct path *old)
|
|
{
|
|
struct path *new;
|
|
|
|
new = path_new(old->r_tool, old->id);
|
|
new->outside = old->outside;
|
|
new->notch = old->notch;
|
|
return new;
|
|
}
|
|
|
|
|
|
static struct point *clone_point(const 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;
|
|
}
|
|
|
|
|
|
int path_is_closed(const struct path *path)
|
|
{
|
|
if (path->first == path->last)
|
|
return 1;
|
|
return points_eq(path->first, path->last);
|
|
}
|
|
|
|
|
|
static void assert_path_is_closed(const struct path *path)
|
|
{
|
|
if (path_is_closed(path))
|
|
return;
|
|
fprintf(stderr, "path from (%g, %g, %g) to (%g, %g, %g) is open\n",
|
|
path->first->x, path->first->y, path->first->z,
|
|
path->last->x, path->last->y, path->last->z);
|
|
abort();
|
|
}
|
|
|
|
|
|
static void path_add_point(struct path *path, struct point *p)
|
|
{
|
|
if (path->last &&
|
|
path->last->x == p->x && path->last->y == p->y &&
|
|
path->last->z == p->z) {
|
|
free(p);
|
|
return;
|
|
}
|
|
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_from(path);
|
|
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;
|
|
}
|
|
|
|
|
|
struct path *path_clone(const struct path *path)
|
|
{
|
|
struct path *new;
|
|
const struct point *p;
|
|
struct point *n;
|
|
|
|
new = path_from(path);
|
|
for (p = path->first; p; p = p->next) {
|
|
n = clone_point(p);
|
|
if (new->first)
|
|
new->last->next = n;
|
|
else
|
|
new->first = n;
|
|
new->last = n;
|
|
}
|
|
return new;
|
|
}
|
|
|
|
|
|
static struct point *offset_point(const struct point *a, const struct point *b,
|
|
const struct point *c, double off, int left)
|
|
{
|
|
double ax, ay, bx, by;
|
|
double aa, bb;
|
|
double nx, ny;
|
|
double angle, f;
|
|
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);
|
|
|
|
if (left) {
|
|
nx = -(ay/aa+by/bb);
|
|
ny = ax/aa+bx/bb;
|
|
} else {
|
|
nx = ay/aa+by/bb;
|
|
ny = -(ax/aa+bx/bb);
|
|
}
|
|
|
|
/* angle between AB and BC */
|
|
angle = acos(-(ax*bx+ay*by)/aa/bb);
|
|
|
|
/* multiplier for combination of normal vectors */
|
|
f = off/sin(angle/2);
|
|
f /= hypot(nx, ny);
|
|
|
|
nx *= f;
|
|
ny *= f;
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
/*
|
|
* Angle in counter-clockwise direction to turn at point B when coming from A
|
|
* in order to face towards C.
|
|
*/
|
|
|
|
static double angle_3(const struct point *a, const struct point *b,
|
|
const struct point *c)
|
|
{
|
|
double ax, ay, bx, by;
|
|
double aa, bb;
|
|
double angle;
|
|
|
|
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);
|
|
|
|
angle = acos((ax*bx+ay*by)/aa/bb)/M_PI*180.0;
|
|
|
|
return (ax*by-ay*bx) >= 0 ? angle : -angle;
|
|
}
|
|
|
|
|
|
/*
|
|
* If we predominantly turn to the right, then the tool must be on the
|
|
* left-hand side. Otherwise, it's on the right.
|
|
*/
|
|
|
|
int path_tool_is_left(const struct path *path)
|
|
{
|
|
const struct point *prev, *p, *next;
|
|
double a = 0;
|
|
|
|
|
|
assert_path_is_closed(path);
|
|
prev = path->first;
|
|
for (p = path->first->next; p; p = p->next) {
|
|
next = p->next ? p->next : path->first->next;
|
|
a += angle_3(prev, p, next);
|
|
prev = p;
|
|
}
|
|
return a < 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 left, int notch)
|
|
{
|
|
struct path *new;
|
|
const struct point *prev, *p, *next;
|
|
struct point *n, *n2;
|
|
int dog;
|
|
|
|
assert_path_is_closed(path);
|
|
if (path->first == path->last)
|
|
return circle(path->first->x, path->first->y, path->first->z,
|
|
path->r_tool, path->r_tool, 0.1, path->id);
|
|
new = path_from(path);
|
|
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, left);
|
|
dog = notch && left_turn(prev, p, next) == left;
|
|
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;
|
|
}
|
|
|
|
|
|
const struct path *path_find_leftmost(const struct path *path)
|
|
{
|
|
const struct point *p;
|
|
const struct path *best = NULL;
|
|
double best_x = HUGE_VAL;
|
|
|
|
while (path) {
|
|
for (p = path->first; p; p = p->next)
|
|
if (p->x < best_x) {
|
|
best = path;
|
|
best_x = p->x;
|
|
}
|
|
path = path->next;
|
|
}
|
|
return best;
|
|
}
|
|
|
|
|
|
int path_is_inside(const struct path *a, const struct path *b)
|
|
{
|
|
struct p2d *pa, *pb;
|
|
int res;
|
|
|
|
pa = path_to_poly(a);
|
|
pb = path_to_poly(b);
|
|
res = p2d_contains_poly(pb, pa);
|
|
p2d_free(pa);
|
|
p2d_free(pb);
|
|
|
|
return res == 1; /* 0 if they intersect */
|
|
}
|
|
|
|
|
|
void path_stats(const struct path *path)
|
|
{
|
|
int paths = 0, segs = 0;
|
|
double len = 0;
|
|
const struct point *p;
|
|
|
|
while (path) {
|
|
paths++;
|
|
for (p = path->first; p; p = p->next) {
|
|
if (!p->next)
|
|
continue;
|
|
segs++;
|
|
len += hypot(hypot(p->x-p->next->x, p->y-p->next->y),
|
|
p->z-p->next->z);
|
|
}
|
|
path = path->next;
|
|
}
|
|
fprintf(stderr, "%d path%s, %d segment%s, %f mm\n",
|
|
paths, paths == 1 ? "" : "s", segs, segs == 1 ? "" : "s", len);
|
|
}
|