Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
592 lines (501 sloc) 10.5 KB
/** \file
* Topologically sort vectors for faster laser cutting or plotting.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
typedef struct _vector vector_t;
struct _vector
{
vector_t * next;
vector_t ** prev;
double x1;
double y1;
double x2;
double y2;
};
typedef struct
{
vector_t * vectors;
} vectors_t;
// Red/Green/Blue
#define VECTOR_PASSES 3
// close enough for floating point work
static inline int fpeq(double x, double y)
{
const double eps = 1e-8;
return fabs(x-y) < eps;
}
static double
vector_transit_len(
vector_t * v
)
{
double lx = 0;
double ly = 0;
double transit_len_sum = 0;
while (v)
{
double t_dx = lx - v->x1;
double t_dy = ly - v->y1;
double transit_len = sqrt(t_dx * t_dx + t_dy * t_dy);
if (transit_len != 0)
transit_len_sum += transit_len;
// Advance the point
lx = v->x2;
ly = v->y2;
v = v->next;
}
return transit_len_sum;
}
static void
vector_stats(
vector_t * v
)
{
double lx = 0;
double ly = 0;
double cut_len_sum = 0;
int cuts = 0;
double transit_len_sum = 0;
int transits = 0;
while (v)
{
double t_dx = lx - v->x1;
double t_dy = ly - v->y1;
double transit_len = sqrt(t_dx * t_dx + t_dy * t_dy);
if (transit_len != 0)
{
transits++;
transit_len_sum += transit_len;
}
double c_dx = v->x1 - v->x2;
double c_dy = v->y1 - v->y2;
double cut_len = sqrt(c_dx*c_dx + c_dy*c_dy);
if (cut_len != 0)
{
cuts++;
cut_len_sum += cut_len;
}
// Advance the point
lx = v->x2;
ly = v->y2;
v = v->next;
}
fprintf(stderr, "Cuts: %u len %.0f\n", cuts, cut_len_sum);
fprintf(stderr, "Move: %u len %.0f\n", transits, transit_len_sum);
}
static void
vector_create(
vectors_t * const vectors,
double x1,
double y1,
double x2,
double y2
)
{
// Find the end of the list and, if vector optimization is
// turned on, check for duplicates
vector_t ** iter = &vectors->vectors;
while (*iter)
{
vector_t * const p = *iter;
if (fpeq(p->x1,x1) && fpeq(p->y1,y1)
&& fpeq(p->x2,x2) && fpeq(p->y2,y2))
return;
if (fpeq(p->x1,x2) && fpeq(p->y1,y2)
&& fpeq(p->x2,x1) && fpeq(p->y2,y1))
return;
if (fpeq(x1,x2) && fpeq(y1,y2))
return;
iter = &p->next;
}
vector_t * const v = calloc(1, sizeof(*v));
if (!v)
return;
v->x1 = x1;
v->y1 = y1;
v->x2 = x2;
v->y2 = y2;
// Append it to the now known end of the list
v->next = NULL;
v->prev = iter;
*iter = v;
}
/**
* Generate a list of vectors.
*
* The vector format is:
* P r,g,b -- Color of the vector
* M x,y -- Move (start a line at x,y)
* L x,y -- Line to x,y from the current position
* Z -- Closing line segment to the starting position
*
* Multi segment vectors are split into individual vectors, which are
* then passed into the topological sort routine.
*
* Exact duplictes will be deleted to try to avoid double hits..
*/
static vectors_t *
vectors_parse(
FILE * const vector_file
)
{
vectors_t * const vectors = calloc(VECTOR_PASSES, sizeof(*vectors));
double mx = 0, my = 0;
double lx = 0, ly = 0;
int pass = 0;
int count = 0;
char buf[256];
while (fgets(buf, sizeof(buf), vector_file))
{
//fprintf(stderr, "read '%s'\n", buf);
const char cmd = buf[0];
double x, y;
switch (cmd)
{
case 'P':
{
// note that they will be in bgr order in the file
int r, g, b;
sscanf(buf+1, "%d %d %d", &b, &g, &r);
if (r == 0 && g != 0 && b == 0)
{
pass = 0;
} else
if (r != 0 && g == 0 && b == 0)
{
pass = 1;
} else
if (r == 0 && g == 0 && b != 0)
{
pass = 2;
} else {
fprintf(stderr, "non-red/green/blue vector? %d,%d,%d\n", r, g, b);
exit(-1);
}
break;
}
case 'M':
// Start a new line.
// This also implicitly sets the
// current laser position
sscanf(buf+1, "%lf %lf", &mx, &my);
lx = mx;
ly = my;
break;
case 'L':
// Add a line segment from the current
// point to the new point, and update
// the current point to the new point.
sscanf(buf+1, "%lf %lf", &x, &y);
vector_create(&vectors[pass], lx, ly, x, y);
count++;
lx = x;
ly = y;
break;
case 'Z':
// Closing segment from the current point
// back to the starting point
vector_create(&vectors[pass], lx, ly, mx, my);
lx = mx;
lx = my;
break;
case 'X':
goto done;
default:
fprintf(stderr, "Unknown command '%c'", cmd);
return NULL;
}
}
done:
fprintf(stderr, "read %u segments\n", count);
/*
for (int i = 0 ; i < VECTOR_PASSES ; i++)
{
vector_stats(vectors[i].vectors);
}
fprintf(stderr, "---\n");
*/
return vectors;
}
/** Find the closest vector to a given point and remove it from the list.
*
* This might reverse a vector if it is closest to draw it in reverse
* order.
*/
static vector_t *
vector_find_closest(
vector_t * v,
const double cx,
const double cy
)
{
double best_dist = 1e9;
vector_t * best = NULL;
int do_reverse = 0;
while (v)
{
double dx1 = cx - v->x1;
double dy1 = cy - v->y1;
double dist1 = dx1*dx1 + dy1*dy1;
if (dist1 < best_dist)
{
best = v;
best_dist = dist1;
do_reverse = 0;
}
double dx2 = cx - v->x2;
double dy2 = cy - v->y2;
double dist2 = dx2*dx2 + dy2*dy2;
if (dist2 < best_dist)
{
best = v;
best_dist = dist2;
do_reverse = 1;
}
v = v->next;
}
if (!best)
return NULL;
// Remove it from the list
if (best->prev)
*best->prev = best->next;
if (best->next)
best->next->prev = best->prev;
// If reversing is required, flip the x1/x2 and y1/y2
if (do_reverse)
{
double x1 = best->x1;
double y1 = best->y1;
best->x1 = best->x2;
best->y1 = best->y2;
best->x2 = x1;
best->y2 = y1;
}
best->next = NULL;
best->prev = NULL;
return best;
}
/**
* Optimize the cut order to minimize transit time.
*
* Simplistic greedy algorithm: look for the closest vector that starts
* or ends at the same point as the current point.
*
* This does not split vectors.
*/
static vector_t *
vector_optimize(
vector_t ** vectors,
double *cx_ptr,
double *cy_ptr
)
{
vector_t * vs = NULL;
vector_t * vs_tail = NULL;
double cx = *cx_ptr;
double cy = *cy_ptr;
while (*vectors)
{
vector_t * v = vector_find_closest(*vectors, cx, cy);
if (!v)
{
fprintf(stderr, "nothing close?\n");
abort();
}
if (!vs)
{
// Nothing on the list yet
vs = vs_tail = v;
} else {
// Add it to the tail of the list
v->next = NULL;
v->prev = &vs_tail->next;
vs_tail->next = v;
vs_tail = v;
}
// Move the current point to the end of the line segment
cx = v->x2;
cy = v->y2;
}
//vector_stats(vs);
*cx_ptr = cx;
*cy_ptr = cy;
// update the pointers
*vectors = vs;
if (vs)
vs->prev = vectors;
return vs;
}
/*
* Attempt to remove long transits.
* Find the longest transit and attempt to move that point
* to a closer point. Then check to see if it reduces the transit.
* returns the reduction in distance
*/
static double
vector_refine(
vector_t * const vector,
double *cx_ptr,
double *cy_ptr
)
{
const double initial_transit_len = vector_transit_len(vector);
double cx = *cx_ptr;
double cy = *cy_ptr;
// find the longest transit
vector_t * v = vector;
double max_transit = 0;
vector_t * transit_v = NULL;
while (v)
{
double t_dx = cx - v->x1;
double t_dy = cy - v->y1;
double transit_len = sqrt(t_dx * t_dx + t_dy * t_dy);
if (!fpeq(transit_len, 0) && max_transit < transit_len)
{
max_transit = transit_len;
transit_v = v;
}
// Advance the point
cx = v->x2;
cy = v->y2;
v = v->next;
}
if (!transit_v)
{
fprintf(stderr, "no longest transit?\n");
return 0;
}
fprintf(stderr, "Total transit: %.3f\n", initial_transit_len);
fprintf(stderr, "longest transit: %.3f: %.3f,%.3f\n",
max_transit,
transit_v->x1,
transit_v->y1
);
// then find the closest *end point* prior to this transit
vector_t * closest = NULL;
double min_dist = 1e9;
for(vector_t * v = vector ; v != transit_v ; v = v->next)
{
double dx = v->x2 - transit_v->x1;
double dy = v->y2 - transit_v->y1;
double dist = dx*dx + dy*dy;
if (min_dist < dist)
continue;
min_dist = dist;
closest = v;
}
if (!closest)
{
fprintf(stderr, "could not find a close one?\n");
return 0;
}
// move the longest transit destination to come after the
// one closest to it, then re-sort based on that point
// remove transit_v from the list
if (transit_v->next)
transit_v->next->prev = transit_v->prev;
if (transit_v->prev)
*transit_v->prev = transit_v->next;
// re-insert it after the closest one
transit_v->next = closest->next;
transit_v->prev = &closest->next;
closest->next = transit_v;
if (transit_v->next)
transit_v->next->prev = &transit_v->next;
// now sort the ones that come after it
double cx2 = transit_v->x2;
double cy2 = transit_v->y2;
vector_optimize(&transit_v->next, &cx2, &cy2);
const double new_transit_len = vector_transit_len(vector);
fprintf(stderr, "Refine transit %.3f\n", new_transit_len);
*cx_ptr = cx2;
*cy_ptr = cy2;
return initial_transit_len - new_transit_len;
}
static void
output_vector(
FILE * const pjl_file,
const vector_t * v
)
{
double lx = 0;
double ly = 0;
while (v)
{
if (fpeq(v->x1,lx) && fpeq(v->y1,ly))
{
// This is the continuation of a line, so
// just add additional points
fprintf(pjl_file, "L %.3f %.3f\n",
v->x2,
v->y2
);
} else {
// Stop the laser; we need to transit
// and then start the laser as we go to
// the next point. Note initial ";"
fprintf(pjl_file, "\nM %.3f %.3f\nL %.3f %.3f\n",
v->x1,
v->y1,
v->x2,
v->y2
);
}
// Changing power on the fly is not supported for now
// \todo: Check v->power and adjust ZS, XR, etc
// Move to the next vector, updating our current point
lx = v->x2;
ly = v->y2;
v = v->next;
}
fprintf(pjl_file, "\n");
}
static void
generate_vectors(
FILE * const vector_file,
FILE * const pjl_file
)
{
vectors_t * const vectors = vectors_parse(vector_file);
double lx = 0;
double ly = 0;
for (int i = 0 ; i < VECTOR_PASSES ; i++)
{
vectors_t * const vs = &vectors[i];
if (!vs->vectors)
continue;
fprintf(stderr, "Group %d\n", i);
vector_stats(vs->vectors);
vector_optimize(
&vs->vectors,
&lx, &ly
);
vector_stats(vs->vectors);
/*
for(int i = 0 ; i < 8 ; i++)
{
double sx = vs->vectors->x1;
double sy = vs->vectors->y1;
if (vector_refine(vs->vectors, &sx, &sy) <= 0)
break;
lx = sx;
ly = sy;
}
*/
fprintf(pjl_file, "P %d %d %d\n",
i == 0 ? 100 : 0,
i == 1 ? 100 : 0,
i == 2 ? 100 : 0
);
output_vector(pjl_file, vs->vectors);
fprintf(pjl_file, "\n\n");
}
}
int main(void)
{
generate_vectors(stdin, stdout);
return 0;
}