mirror of
git://projects.qi-hardware.com/cae-tools.git
synced 2024-12-23 12:01:08 +02:00
345 lines
6.3 KiB
C
345 lines
6.3 KiB
C
|
#include <stdlib.h>
|
||
|
#include <stdio.h>
|
||
|
#include <string.h>
|
||
|
#include <math.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;
|
||
|
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 const 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;
|
||
|
}
|
||
|
|
||
|
|
||
|
static const struct vertex *corner(struct path *path, const struct vertex *a,
|
||
|
const struct vertex *b, const struct vertex *c, double r, double d)
|
||
|
{
|
||
|
double ax = b->x-a->x;
|
||
|
double ay = b->y-a->y;
|
||
|
double bx = c->x-b->x;
|
||
|
double by = c->y-b->y;
|
||
|
double aa = hypot(ax, ay);
|
||
|
double bb = hypot(bx, by);
|
||
|
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 dd; /* "d" of the given vectors */
|
||
|
double tt, s;
|
||
|
double t2, p, q, ang;
|
||
|
double u, v;
|
||
|
double f, x, y;
|
||
|
int n, i;
|
||
|
|
||
|
/*
|
||
|
* 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;
|
||
|
|
||
|
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.
|
||
|
*/
|
||
|
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) {
|
||
|
append_vertex(path, clone_vertex(b));
|
||
|
return b;
|
||
|
}
|
||
|
|
||
|
/* 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));
|
||
|
|
||
|
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
|
||
|
*/
|
||
|
|
||
|
f = s/aa;
|
||
|
x = b->x-f*ax;
|
||
|
y = b->y-f*ay;
|
||
|
add_vertex(path, x, y, b->r, b->d, b->tag);
|
||
|
|
||
|
/*
|
||
|
* 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 should evenly distribute the slack, but that seems
|
||
|
* difficult. For now, we just center the polygon.
|
||
|
*/
|
||
|
q = (t2/2-p)/(n+1);
|
||
|
|
||
|
double dir = copysign(1, cp);
|
||
|
#if 0
|
||
|
if (cp < 0) {
|
||
|
// t2 = -t2;
|
||
|
q = -q;
|
||
|
p = -p;
|
||
|
}
|
||
|
#endif
|
||
|
|
||
|
if (n)
|
||
|
ang = p+q;
|
||
|
else
|
||
|
ang = t2/2;
|
||
|
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);
|
||
|
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
|
||
|
*/
|
||
|
|
||
|
f = s/bb;
|
||
|
return add_vertex(path, b->x+f*bx, b->y+f*by, 0, 0, NULL);
|
||
|
}
|
||
|
|
||
|
|
||
|
struct path *round_path(const struct path *path, double r, double d)
|
||
|
{
|
||
|
struct path *new;
|
||
|
const struct vertex *prev, *v;
|
||
|
|
||
|
new = alloc_path();
|
||
|
prev = path->vertices;
|
||
|
if (!prev)
|
||
|
return new;
|
||
|
append_vertex(new, clone_vertex(prev));
|
||
|
if (!prev->next)
|
||
|
return new;
|
||
|
|
||
|
if (prev->r)
|
||
|
r = prev->r;
|
||
|
if (prev->d)
|
||
|
d = prev->d;
|
||
|
|
||
|
for (v = prev->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;
|
||
|
}
|
||
|
|
||
|
|
||
|
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;
|
||
|
}
|
||
|
|
||
|
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);
|
||
|
}
|
||
|
}
|