1
0
mirror of git://projects.qi-hardware.com/cae-tools.git synced 2024-12-23 09:43:20 +02:00
cae-tools/ptrude/path.c
2011-07-31 19:17:05 -03:00

552 lines
11 KiB
C

/*
* path.c - 2D path operations
*
* Written 2011 by Werner Almesberger
* Copyright 2011 by 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 <string.h>
#include <math.h>
#include <assert.h>
#include "ptrude.h"
#include "path.h"
#define alloc_type(t) ((t *) malloc(sizeof(t)))
#define stralloc(s) strdup(s)
static double deg(double rad)
{
return rad/M_PI*180.0;
}
static struct path *alloc_path(void)
{
struct path *path;
path = alloc_type(struct path);
path->vertices = NULL;
path->last = &path->vertices;
return path;
}
static struct vertex *alloc_vertex(void)
{
struct vertex *v;
v = alloc_type(struct vertex);
v->r = 0;
v->d = 0;
v->tag = NULL;
v->next = NULL;
v->len = 0;
return v;
}
static void free_vertex(struct vertex *v)
{
free(v);
}
void free_path(struct path *path)
{
struct vertex *v, *next;
for (v = path->vertices; v; v = next) {
next = v->next;
free_vertex(v);
}
free(path);
}
static struct vertex *clone_vertex(const struct vertex *v)
{
struct vertex *new;
new = alloc_type(struct vertex);
*new = *v;
new->next = NULL;
return new;
}
static void append_vertex(struct path *path, struct vertex *v)
{
*path->last = v;
path->last = &v->next;
}
static struct vertex *add_vertex(struct path *path, double x, double y,
double r, double d, const char *tag)
{
struct vertex *v;
v = alloc_vertex();
v->x = x;
v->y = y;
v->r = r;
v->d = d;
v->tag = tag;
append_vertex(path, v);
return v;
}
double path_set_length(struct path *path)
{
struct vertex *v;
double sum = 0;
if (!path->vertices)
return 0;
for (v = path->vertices; v->next; v = v->next) {
v->len = hypot(v->x-v->next->x, v->y-v->next->y);
sum += v->len;
}
v->len = 0;
return sum;
}
static void adjust_length(struct vertex *from, struct vertex *to, double len)
{
struct vertex *v;
double sum, f;
if (from == to)
return;
sum = 0;
for (v = from->next; v != to; v = v->next) {
v->len = hypot(v->x-v->next->x, v->y-v->next->y);
sum += v->len;
}
f = len/sum;
for (v = from->next; v != to; v = v->next)
v->len *= f;
}
/*
* "corner" replaces a corner with a ploygon if the corner is too sharp to be
* within distance "d" of the bend radius. This may change the point from
* where we resume drawing (originally the corner point, "b"). "corner"
* therefore returns the new end of the arc.
*/
static struct vertex *corner(struct path *path, struct vertex *a,
const struct vertex *b, const struct vertex *c, double r, double d)
{
/* points to vectors */
double ax = b->x-a->x;
double ay = b->y-a->y;
double bx = c->x-b->x;
double by = c->y-b->y;
/* vector length */
double aa = hypot(ax, ay);
double bb = hypot(bx, by);
/* dot and cross product */
double dp = ax*bx+ay*by; /* a * b = a*b*cos 2t */
double cp = ax*by-ay*bx; /* |a x b| = a*b*sin 2t */
double dir = copysign(1, cp);
/* see corner.fig */
double dd; /* "d" of the given vectors */
double tt; /* tan t */
double s; /* distance between start of arc and corner */
double t2; /* angle, t*2 */
/* see arc.fig */
double p; /* half-angle of border side of border segment */
double q; /* half-angle of connecting segment */
double u; /* length of border side of border segment */
double v; /* half-length of connecting segment */
int n; /* number of connecting segments (0 if none) */
double f; /* scale factor; various uses */
double fa, fb; /* scale factors for first and last vertex */
double ang; /* current angle, for iteration */
double x, y; /* current position; for iteration */
int i; /* segment; for iteration */
struct vertex *v0; /* first vertex of arc */
struct vertex *v1; /* last vertex of arc */
/*
* http://en.wikipedia.org/wiki/Dot_product
* dp = a*b*cos 2t
*
* http://en.wikipedia.org/wiki/Cross_product
* cp = a*b*sin 2t
*
* http://en.wikipedia.org/wiki/Tangent_half-angle_formula
* tan t = sin 2t/(1+cos 2t)
*/
tt = cp/(aa*bb+dp);
/*
* From s = r*tan t
*/
s = fabs(r*tt);
/*
* From r^2+s^2 = (r+d)^2
*/
dd = hypot(r, s)-r;
if (debug) {
fprintf(stderr, "a = (%g, %g)-(%g, %g) = (%g, %g); |a| = %g\n",
b->x, b->y, a->x, a->y, ax, ay, aa);
fprintf(stderr, "b = (%g, %g)-(%g, %g) = (%g, %g); |b| = %g\n",
c->x, c->y, b->x, b->y, bx, by, bb);
fprintf(stderr, "sin 2t = %g, cos 2t = %g, tan t = %g\n",
cp/aa/bb, dp/aa/bb, tt);
fprintf(stderr, "r = %g, d = %g, s = %g, dd = %g\n",
r, d, s, dd);
}
/*
* We only know how to make a rounded corner if two vectors are
* involved. They therefore have to be long enough to accommodate the
* entire arc, from beginning to end. Furthermore, we split the
* available length in half, one for the inbound arc, the other for the
* outbound arc.
*/
/*
* @@@ Our error checking is a bit overzealous and doesn't provide
* enough information to debug any problems. Turn errors into warnings
* for now.
*/
if (aa/2 < s) {
fprintf(stderr, "first vector is too short (%g/2 < %g)\n",
aa, s);
// exit(1);
}
if (bb/2 < s) {
fprintf(stderr, "second vector is too short (%g/2 < %g)\n",
bb, s);
// exit(1);
}
/*
* If the corner is already smooth enough, we just keep what we have.
*/
if (dd <= d) {
v1 = clone_vertex(b);
append_vertex(path, v1);
return v1;
}
/* Step 1: determine the total angle (2*t) */
t2 = acos(dp/aa/bb);
/*
* Step 2: determine the maximum angle of the first and last segment.
*
* We use
* r*cos p = r-d
* cos p = 1-d/r
*/
p = acos(1-d/r);
/*
* Step 3: determine the maximum angle of intermediate segments (if
* there are any).
*
* We use
* (r+d)*cos q = r-d
* cos q = r-q/(r+d)
*/
q = acos((r-d)/(r+d));
if (debug)
fprintf(stderr, "t2 = %g, p(max) = %g, q(max) = %g\n",
deg(t2), deg(p), deg(q));
/*
* Step 4: emit the starting point of the arc
*/
fa = s/aa;
x = b->x-fa*ax;
y = b->y-fa*ay;
v0 = add_vertex(path, x, y, b->r, b->d, b->tag);
v0->len = a->len*(1-fa);
/*
* Step 5: determine if we need intermediate points. If yes, how many,
* and then proceed to add them.
*/
if (t2 > 2*p) {
n = (int) ceil((t2-2*(p+q))/(2*q));
/*
* We could evenly distribute the slack and try to pick a
* smaller value for d, but that seems difficult.
*
* A drawback of reducing p would be that we may make the
* corner unnecessarily sharp, possibly even turning against
* the general direction of the turn. We'd still respect the
* bend radius and the tolerance, but the result may look weird
* anyway.
*
* For now, we just center the polygon.
*/
q = (t2/2-p)/(n+1);
if (n)
ang = p+q;
else {
ang = t2/2;
/*
* @@@ To do: adjust the radius such that we always hug
* the r-d circle (see arc.fig) and usually not the
* r+d circle. Right now, it's just the opposite.
*/
}
u = tan(p)*(r-d);
v = tan(q)*(r-d);
f = (u+v)/aa;
for (i = 0; i <= n; i++) {
x += f*ax*cos(ang-q)-dir*f*ay*sin(ang-q);
y += dir*f*ax*sin(ang-q)+f*ay*cos(ang-q);
if (debug)
fprintf(stderr, " %d/%d: %g %g @ %g\n", i, n,
x, y, deg(ang));
add_vertex(path, x, y, 0, 0, NULL);
ang += 2*q;
f = (2*v)/aa;
}
}
/*
* Step 6: emit the finishing point of the arc
*/
fb = s/bb;
v1 = add_vertex(path, b->x+fb*bx, b->y+fb*by, 0, 0, NULL);
v1->len = b->len*(1-fb);
/*
* Step 7: adjust the nominal length of the segments
*/
adjust_length(v0, v1, a->len*fa+b->len*fb);
return v1;
}
struct path *round_path(const struct path *path, double r, double d)
{
struct path *new;
struct vertex *prev;
const struct vertex *v;
new = alloc_path();
if (!path->vertices)
return new;
prev = clone_vertex(path->vertices);
append_vertex(new, prev);
if (!path->vertices->next)
return new;
if (prev->r)
r = prev->r;
if (prev->d)
d = prev->d;
for (v = path->vertices->next; v->next; v = v->next) {
if (v->r)
r = v->r;
if (v->d)
d = v->d;
prev = corner(new, prev, v, v->next, r, d);
}
append_vertex(new, clone_vertex(v));
return new;
}
static void move_vertex(struct path *path, const struct vertex *v,
double nx, double ny, double dist, double r)
{
struct vertex *new;
new = clone_vertex(v);
new->x += nx*dist;
new->y += ny*dist;
new->r = r;
append_vertex(path, new);
}
struct path *stretch_path(const struct path *path, double dist, double r)
{
struct path *new; /* new path */
const struct vertex *v; /* current vertex (for iteration) */
const struct vertex *a, *b, *c; /* previous, current, next vertex */
double nx, ny; /* 2D normals */
double f; /* factor for normalization */
double tx, ty; /* temporary 2D normals */
new = alloc_path();
a = path->vertices;
b = a->next;
nx = b->y-a->y;
ny = a->x-b->x;
f = hypot(nx, ny);
if (a->r)
r = a->r;
move_vertex(new, a, nx/f, ny/f, dist, r);
for (v = path->vertices->next; v->next; v = v->next) {
double tmp;
b = v;
c = v->next;
tx = b->y-a->y;
ty = a->x-b->x;
f = hypot(tx, ty);
nx = tx/f;
ny = ty/f;
tmp = f;
tx = c->y-b->y;
ty = b->x-c->x;
f = hypot(tx, ty);
nx += tx/f;
ny += ty/f;
if (b->r)
r = b->r;
f = hypot(nx, ny);
nx /= f;
ny /= f;
/*
* We have this far:
* nx, ny = normal on corner, normalized
* tmp = |a|, length of vector "a" (A -> B)
* dist = the distance by which we stretch
*
* As shown in stretch.fig, we the length we need is
* d' = d/cos(90-t)
*
* With
* http://en.wikipedia.org/wiki/Trigonometric_identities#Symmetry
* cos(90-t) = sin t = (n x a)/(|n|*|a|)
*
* Thus
* d' = d/sin(t) - d*(|n|*|a|)/(n x a)
* = d/sin(t) - d*|a|/(n x a)
*/
tmp = dist*tmp/(nx*(b->y-a->y)-ny*(b->x-a->x));
move_vertex(new, b, nx, ny, tmp, r+dist);
a = v;
}
nx = v->y-a->y;
ny = a->x-v->x;
f = hypot(nx, ny);
if (v->r)
r = v->r;
move_vertex(new, v, nx/f, ny/f, dist, r);
return new;
}
struct path *load_path(FILE *file)
{
struct path *path;
char buf[1100]; /* plenty :) */
char buf2[sizeof(buf)];
char *s;
float x, y, tmp;
float r = 0, d = 0;
const char *tag = NULL;
path = alloc_path();
while (fgets(buf, sizeof(buf),file)) {
s = strchr(buf, '\n');
if (s)
*s = 0;
if (sscanf(buf, "#r=%f", &tmp) == 1) {
r = tmp;
continue;
}
if (sscanf(buf, "#delta=%f", &tmp) == 1) {
d = tmp;
continue;
}
if (sscanf(buf, "#tag=%s", buf2) == 1) {
tag = stralloc(buf2);
continue;
}
if (*buf == '#')
continue;
if (sscanf(buf, "%f %f", &x, &y) != 2) {
fprintf(stderr, "can't parse \"%s\"\n", buf);
exit(1);
}
add_vertex(path, x, y, r, d, tag);
r = 0;
d = 0;
tag = NULL;
}
path_set_length(path);
return path;
}
void save_path(FILE *file, const struct path *path)
{
const struct vertex *v;
for (v = path->vertices; v; v = v->next) {
if (v->r)
fprintf(file, "#r=%f\n", v->r);
if (v->d)
fprintf(file, "#delta=%f\n", v->d);
if (v->tag)
fprintf(file, "#delta=%f\n", v->d);
fprintf(file, "%f %f\n", v->x, v->y);
}
}