cae-tools/cameo/area.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);
	}
}