459 lines
9.4 KiB
C
459 lines
9.4 KiB
C
/*
|
|
* area.c - Area fill
|
|
*
|
|
* Written 2012 by Werner Almesberger
|
|
* Copyright 2012 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.
|
|
*/
|
|
|
|
/*
|
|
* We use the following requirement to simplify toolpath generation: the
|
|
* outlines must be designed such that the tool can pass along all the
|
|
* outlines without cutting into anything it's not supposed to.
|
|
*/
|
|
|
|
#include <stddef.h>
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
|
|
#include "util.h"
|
|
#include "path.h"
|
|
#include "area.h"
|
|
|
|
|
|
#define EPSILON 1e-6
|
|
|
|
|
|
static int bbox(const struct path *path,
|
|
double *xa, double *ya, double *xb, double *yb)
|
|
{
|
|
const struct point *p = path->first;
|
|
|
|
if (!p)
|
|
return 0;
|
|
*xa = *xb = p->x;
|
|
*ya = *yb = p->y;
|
|
while (p) {
|
|
if (p->x < *xa)
|
|
*xa = p->x;
|
|
if (p->x > *xb)
|
|
*xb = p->x;
|
|
if (p->y < *ya)
|
|
*ya = p->y;
|
|
if (p->y > *yb)
|
|
*yb = p->y;
|
|
p = p->next;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*
|
|
* @@@ this is a bit too simple. E.g., it would report A as being inside B
|
|
* in this case:
|
|
*
|
|
* +---+
|
|
* +---+ | |
|
|
* | A | | |
|
|
* +---+ | |
|
|
* | B |
|
|
* +--------+ |
|
|
* | |
|
|
* +------------+
|
|
*/
|
|
|
|
static int is_inside(const struct path *a, const struct path *b)
|
|
{
|
|
double xa, ya, xb, yb;
|
|
const struct point *p;
|
|
|
|
if (!bbox(b, &xa, &ya, &xb, &yb))
|
|
return 0;
|
|
for (p = a->first; p; p = p->next)
|
|
if (p->x < xa || p->x > xb ||
|
|
p->y < ya || p->y > yb)
|
|
return 0;
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*
|
|
* Solve
|
|
*
|
|
* ax+by = e
|
|
* cx+dy = f
|
|
*
|
|
* with Cramer's rule:
|
|
* http://en.wikipedia.org/wiki/Cramer's_rule
|
|
*/
|
|
|
|
static int cramer2(double a, double b, double c, double d, double e, double f,
|
|
double *x, double *y)
|
|
{
|
|
double det;
|
|
|
|
det = a*d-b*c;
|
|
if (fabs(det) < EPSILON)
|
|
return 0;
|
|
*x = (e*d-b*f)/det;
|
|
*y = (a*f-e*c)/det;
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*
|
|
* Solve
|
|
*
|
|
* ax + na*bx = cx + nb*dx
|
|
* ay + na*by = cy + nb*dy
|
|
*
|
|
* which is
|
|
*
|
|
* na*bx + nb*-dx = cx - ax
|
|
* na*by + nb*-dy = cy - ay
|
|
*/
|
|
|
|
static int intersect(double ax, double ay, double bx, double by,
|
|
double cx, double cy, double dx, double dy, double *na, double *nb)
|
|
{
|
|
return cramer2(bx, -dx, by, -dy, cx-ax, cy-ay, na, nb);
|
|
}
|
|
|
|
|
|
/*
|
|
* See above.fig. The equation we solve is
|
|
*
|
|
* Q = A+u*(AM)
|
|
* Q = B+v*(BP)
|
|
*
|
|
* equals
|
|
*
|
|
* ax + u*(mx-ax) = bx + v*(px-bx)
|
|
* ay + u*(my-ay) = by + v*(py-by)
|
|
*
|
|
* equals
|
|
*
|
|
* u*(mx-ax) + v*(bx-px) = bx - ax
|
|
* u*(my-ay) + v*(by-py) = by - ay
|
|
*
|
|
* For BC, the equation becomes
|
|
*
|
|
* Q = C+u*(CM)
|
|
* Q = B+v*(BP)
|
|
*/
|
|
|
|
static int above(const struct point *a, const struct point *b,
|
|
const struct point *c, double px, double py)
|
|
{
|
|
double ab, bc;
|
|
double mx, my;
|
|
double u, v;
|
|
|
|
ab = hypot(a->x-b->x, a->y-b->y);
|
|
bc = hypot(b->x-c->x, b->y-c->y);
|
|
if (fabs(ab) < EPSILON || fabs(bc) < EPSILON)
|
|
return 0;
|
|
|
|
mx = b->x-(b->y-a->y)/ab-(c->y-b->y)/bc;
|
|
my = b->y+(b->x-a->x)/ab+(c->x-b->x)/bc;
|
|
|
|
if (cramer2(mx-a->x, b->x-px, my-a->y, b->y-py, b->x-a->x, b->y-a->y,
|
|
&u, &v))
|
|
if (u >= 0 && u <= 1 && v >= 0)
|
|
return 1;
|
|
if (cramer2(mx-c->x, b->x-px, my-c->y, b->y-py, b->x-c->x, b->y-c->y,
|
|
&u, &v))
|
|
if (u >= 0 && u <= 1 && v >= 0)
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*
|
|
* Solve
|
|
*
|
|
* (ax+n*bx-cx)^2+(ay+n*by-cy)^2 = r^2 for n
|
|
*
|
|
* http://en.wikipedia.org/wiki/Quadratic_equation
|
|
*/
|
|
|
|
static int touch_solve(double ax, double ay, double bx, double by,
|
|
double cx, double cy, double r, int enter, double *n)
|
|
{
|
|
double dx = cx-ax;
|
|
double dy = cy-ay;
|
|
double a = bx*bx+by*by; /* always positive */
|
|
double b = -2*bx*dx-2*by*dy;
|
|
double c = dx*dx+dy*dy-r*r;
|
|
double d, tmp;
|
|
|
|
d = b*b-4*a*c;
|
|
if (d < 0)
|
|
return 0;
|
|
d = sqrt(d);
|
|
tmp = enter ? (-b-d)/2/a : (-b+d)/2/a;
|
|
if (tmp <= 0 || tmp >= 1)
|
|
return 0;
|
|
*n = tmp;
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*
|
|
* The points A, B, and C are (if the path is left-handed):
|
|
*
|
|
* - A: the beginning of the segment leading into the corner
|
|
* - B: the corner point
|
|
* - C: the beginning of the segment leading out of the corner
|
|
*
|
|
* If the path is right-handed, we swap A and C, making it left-handed.
|
|
*/
|
|
|
|
static int touch(double ax, double ay, double bx, double by,
|
|
const struct point *a, const struct point *b, const struct point *c,
|
|
double r, int enter, int left, double *n)
|
|
{
|
|
double px, py;
|
|
|
|
if (!touch_solve(ax, ay, bx, by, b->x, b->y, r, enter, n))
|
|
return 0;
|
|
px = ax+*n*bx;
|
|
py = ay+*n*by;
|
|
return above(a, b, c, px, py) == left;
|
|
}
|
|
|
|
|
|
/*
|
|
* Here, the points A, B, C, and D are:
|
|
*
|
|
* - A: before the beginning of the current segment
|
|
* - B: the beginning
|
|
* - C: the end
|
|
* - D: the next point beyond the end
|
|
*/
|
|
|
|
static int hit_segment(double fx, double fy, double tx, double ty,
|
|
const struct point *a, const struct point *b, const struct point *c,
|
|
const struct point *d, double r, int enter, int left, double *n)
|
|
{
|
|
double dx, dy, nx, ny, nn;
|
|
double px, py;
|
|
double na, nb;
|
|
|
|
tx -= fx;
|
|
ty -= fy;
|
|
|
|
dx = c->x-b->x;
|
|
dy = c->y-b->y;
|
|
|
|
if (left) {
|
|
nx = dx;
|
|
ny = dy;
|
|
} else {
|
|
nx = -dx;
|
|
ny = -dy;
|
|
}
|
|
|
|
/* -dy becomes the x component of the normal vector */
|
|
if (enter ? ny < 0 : ny > 0)
|
|
return 0;
|
|
|
|
nn = hypot(nx, ny);
|
|
|
|
px = b->x-ny/nn*r;
|
|
py = b->y+nx/nn*r;
|
|
|
|
if (!intersect(fx, fy, tx, ty, px, py, dx, dy, &na, &nb))
|
|
return 0;
|
|
if (nb <= 0) {
|
|
if (!touch(fx, fy, tx, ty, a, b, c, r, enter, left, &na))
|
|
return 0;
|
|
}
|
|
if (nb >= 1) {
|
|
if (!touch(fx, fy, tx, ty, b, c, d, r, enter, left, &na))
|
|
return 0;
|
|
}
|
|
if (na <= 0 || na >= 1)
|
|
return 0;
|
|
*n = na;
|
|
return 1;
|
|
}
|
|
|
|
|
|
static int hit_path(double fx, double fy, double tx, double ty,
|
|
const struct path *path, int inside, int enter, double r, double *x)
|
|
{
|
|
const struct point *p, *last, *next2;
|
|
int left;
|
|
double nx, tmp;
|
|
int found = 0;
|
|
|
|
/*
|
|
* @@@ We don't wrap around the ends properly and create a zero-sized
|
|
* imaginary segment between path->first and path->last.
|
|
*/
|
|
left = path_tool_is_left(path);
|
|
if (inside)
|
|
left = !left;
|
|
last = path->last;
|
|
for (p = path->first; p != path->last; p = p->next) {
|
|
next2 = p->next->next ? p->next->next : path->first;
|
|
if (hit_segment(fx, fy, tx, ty, last, p, p->next, next2,
|
|
r, enter, left, &tmp)) {
|
|
if (!found || nx > tmp)
|
|
nx = tmp;
|
|
found = 1;
|
|
}
|
|
last = p;
|
|
}
|
|
if (found)
|
|
*x = fx+nx*(tx-fx);
|
|
return found;
|
|
}
|
|
|
|
|
|
static const struct path **subordinates(const struct path *paths,
|
|
const struct path *path, double z)
|
|
{
|
|
const struct path **sub, **w, **a, **b;;
|
|
const struct path *p;
|
|
int n = 0;
|
|
|
|
for (p = paths; p; p = p->next)
|
|
if (p->first && p->first->z == z)
|
|
n++;
|
|
sub = alloc_size(sizeof(struct path *)*n);
|
|
w = sub;
|
|
for (p = paths; p; p = p->next)
|
|
if (p != path && p->first && p->first->z == z &&
|
|
is_inside(p, path) && !is_inside(path, p))
|
|
*w++ = p;
|
|
*w = NULL;
|
|
for (a = sub; a != w; a++)
|
|
for (b = sub; b != w; b++)
|
|
if (a != b && is_inside(*a, *b)) {
|
|
*a = *--w;
|
|
*w = NULL;
|
|
a--;
|
|
break;
|
|
}
|
|
return sub;
|
|
}
|
|
|
|
|
|
static void do_line(const struct path *path, const struct path **sub,
|
|
double xa, double xb, double y, double r_tool, double overlap,
|
|
struct path **res)
|
|
{
|
|
const struct path *last = path;
|
|
const struct path **s;
|
|
struct path *new;
|
|
double x, next;
|
|
|
|
if (!hit_path(xa-3*r_tool, y, xb, y, last, 1, 0, r_tool, &x))
|
|
return;
|
|
while (1) {
|
|
next = xb;
|
|
last = NULL;
|
|
if (hit_path(x, y, xb, y, path, 1, 1, r_tool, &next))
|
|
last = path;
|
|
for (s = sub; *s; s++)
|
|
if (hit_path(x, y, next, y, *s, 0, 1, r_tool, &next))
|
|
last = *s;
|
|
if (next-x > 2*r_tool-2*overlap) {
|
|
new = path_new(r_tool, "");
|
|
path_add(new, x+r_tool-overlap, y, path->first->z);
|
|
path_add(new, next-r_tool+overlap, y, path->first->z);
|
|
new->next = *res;
|
|
*res = new;
|
|
}
|
|
if (!last)
|
|
return;
|
|
if (!hit_path(next+EPSILON, y, xb, y, last, last == path, 0,
|
|
r_tool, &x))
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
static void add_outline(const struct path *path, int inside, struct path **res)
|
|
{
|
|
struct path *new;
|
|
int left;
|
|
|
|
left = path_tool_is_left(path);
|
|
new = path_offset(path, inside ? !left : left, 0);
|
|
new->next = *res;
|
|
*res = new;
|
|
}
|
|
|
|
|
|
static void fill_path(const struct path *paths, const struct path *path,
|
|
double z, double r_tool, double overlap, struct path **res)
|
|
{
|
|
const struct path **sub, **s;
|
|
const struct path **sub2, **s2;
|
|
double xa, ya, xb, yb;
|
|
int n, i;
|
|
|
|
if (!bbox(path, &xa, &ya, &xb, &yb))
|
|
return;
|
|
sub = subordinates(paths, path, z);
|
|
xa += r_tool;
|
|
ya += 3*r_tool-overlap;
|
|
xb -= r_tool;
|
|
yb -= 3*r_tool-overlap;
|
|
n = ceil((yb-ya)/(2*r_tool-overlap));
|
|
for (i = 0; i <= n; i++)
|
|
do_line(path, sub, xa, xb, ya+(yb-ya)*((double) i/n),
|
|
r_tool, overlap, res);
|
|
for (s = sub; *s; s++) {
|
|
sub2 = subordinates(paths, *s, z);
|
|
for (s2 = sub2; *s2; s2++)
|
|
fill_path(paths, *s2, z, r_tool, overlap, res);
|
|
free(sub2);
|
|
add_outline(*s, 0, res);
|
|
}
|
|
free(sub);
|
|
add_outline(path, 1, res);
|
|
}
|
|
|
|
|
|
struct path *area(const struct path *paths, double overlap)
|
|
{
|
|
struct path *res = NULL;
|
|
double z = HUGE_VAL, best_x, x;
|
|
const struct path *path, *best;
|
|
const struct point *p;
|
|
|
|
if (!paths)
|
|
return NULL;
|
|
while (1) {
|
|
best = NULL;
|
|
best_x = HUGE_VAL;
|
|
for (path = paths; path; path = path->next) {
|
|
if (!path->first)
|
|
continue;
|
|
if (path->first->z >= z)
|
|
continue;
|
|
x = HUGE_VAL;
|
|
for (p = path->first; p; p = p->next)
|
|
if (p->x < x)
|
|
x = p->x;
|
|
if (best && best->first->z >= path->first->z &&
|
|
x >= best_x)
|
|
continue;
|
|
best = path;
|
|
best_x = x;
|
|
}
|
|
if (!best)
|
|
return res;
|
|
z = best->first->z;
|
|
fill_path(paths, best, z, best->r_tool, overlap, &res);
|
|
}
|
|
}
|