From bb14499ee3310d60dfcfe122344ba82954a1ce9d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sun, 20 Sep 2020 16:38:05 +0200 Subject: [PATCH] lsynth: Import 3.1 source. --- lsynth/src/band.c | 1415 ++++++++++++++++++++++++++ lsynth/src/band.h | 73 ++ lsynth/src/curve.c | 1038 +++++++++++++++++++ lsynth/src/curve.h | 44 + lsynth/src/curvy.c | 440 +++++++++ lsynth/src/hose.c | 889 +++++++++++++++++ lsynth/src/hose.h | 50 + lsynth/src/lsynthcp.c | 2084 +++++++++++++++++++++++++++++++++++++++ lsynth/src/lsynthcp.h | 73 ++ lsynth/src/lsynthcp.res | Bin 0 -> 876 bytes lsynth/src/makefile | 17 + lsynth/src/mathlib.c | 324 ++++++ lsynth/src/mathlib.h | 25 + lsynth/src/orient.c | 94 ++ lsynth/src/quat.c | 603 +++++++++++ lsynth/src/strings.c | 43 + lsynth/src/strings.h | 21 + 17 files changed, 7233 insertions(+) create mode 100644 lsynth/src/band.c create mode 100644 lsynth/src/band.h create mode 100644 lsynth/src/curve.c create mode 100644 lsynth/src/curve.h create mode 100644 lsynth/src/curvy.c create mode 100644 lsynth/src/hose.c create mode 100644 lsynth/src/hose.h create mode 100644 lsynth/src/lsynthcp.c create mode 100644 lsynth/src/lsynthcp.h create mode 100644 lsynth/src/lsynthcp.res create mode 100644 lsynth/src/makefile create mode 100644 lsynth/src/mathlib.c create mode 100644 lsynth/src/mathlib.h create mode 100644 lsynth/src/orient.c create mode 100644 lsynth/src/quat.c create mode 100644 lsynth/src/strings.c create mode 100644 lsynth/src/strings.h diff --git a/lsynth/src/band.c b/lsynth/src/band.c new file mode 100644 index 000000000..ee3711dd2 --- /dev/null +++ b/lsynth/src/band.c @@ -0,0 +1,1415 @@ +/* + * This is the LDRAW parts synthesis library. + * By Kevin Clague and Don Heyse + */ + +#include +#include + +#include "lsynthcp.h" +#include "band.h" +#include "hose.h" + +/* + * 0 SYNTH BEGIN DEFINE BAND RUBBER_BAND "Descr" + * 1 a b c d e f g h i j k l "name" + * 1 a b c d e f g h i j k l "name" + * 0 SYNTH END + */ + +int n_band_types; +band_attrib_t band_types[32]; + +#define N_BAND_TYPES n_band_types + +/* + * 0 SYNTH BEGIN DEFINE BAND CONSTRAINTS + * 1 a b c d e f g h i j k l "name" + * 0 SYNTH END + */ + +int n_band_constraints; +part_t band_constraints[64]; + +#define N_BAND_CONSTRAINTS n_band_constraints + +// Make current band_type a global var so we can use it everywhere. +band_attrib_t *band_type = NULL; + +/************************************************************************/ +// Return 1 if the v2 bends left of v1, -1 if right, 0 if straight ahead. +int turn_vector(PRECISION v1[3], PRECISION v2[3]) +{ + /* Pos for left bend, 0 = linear */ + PRECISION vec_product = (v1[0] * v2[1]) - (v1[1] * v2[0]); + + if (vec_product > 0.0) return(1); + if (vec_product < 0.0) return(-1); + return(0); +} + + //********************************************************************** +void band_ini(void) +{ + int i; + + for (i = 0; i < N_BAND_TYPES; i++) { + printf("%-20s = SYNTH BEGIN %s 16\n",band_types[i].type, band_types[i].type); + } +} + +void +list_band_types(void) +{ + int i; + + printf("\n\nBand type synthesizable parts\n"); + for (i = 0; i < N_BAND_TYPES; i++) { + printf(" %-20s %s\n",band_types[i].type, band_types[i].descr); + } +} + +int +isbandtype(char *type) +{ + int i; + + for (i = 0; i < N_BAND_TYPES; i++) { + if (strncasecmp(band_types[i].type,type,strlen(band_types[i].type)) == 0) { + return 1; + } + } + return 0; +} + +int +isbandconstraint(char *type) +{ + int i; + + for (i = 0; i < N_BAND_CONSTRAINTS; i++) { + if (strcasecmp(band_constraints[i].type,type) == 0) { + return 1; + } + } + return 0; +} + +void +list_band_constraints(void) +{ + int i; + + printf("\n\nBand type synthesis constraints\n"); + for (i = 0; i < N_BAND_CONSTRAINTS; i++) { + printf(" %11s\n",band_constraints[i].type); + } +} + + /* + * Calculate the issues of crossers + */ + +void +calc_crosses( + LSL_band_constraint *k, + LSL_band_constraint *m, + int *layer, + FILE *output) +{ + PRECISION xlk = k->end_line[0] - k->start_line[0]; + PRECISION ylk = k->end_line[1] - k->start_line[1]; + PRECISION xnm = m->end_line[0] - m->start_line[0]; + PRECISION ynm = m->end_line[1] - m->start_line[1]; + PRECISION xmk = m->start_line[0] - k->start_line[0]; + PRECISION ymk = m->start_line[1] - k->start_line[1]; + + PRECISION det = xnm*ylk - ynm*xlk; + if (fabs(det) < ACCY) { + /* parallel lines */ + } else { + PRECISION detinv = 1.0/det; + PRECISION s = (xnm*ymk - ynm*xmk)*detinv; + PRECISION t = (xlk*ymk - ylk*xmk)*detinv; + if (s >= 0 && s <= 1.0 && t >= 0 && t <= 1.0) { + PRECISION x = k->start_line[0] + xlk*s; + PRECISION y = k->start_line[1] + ylk*s; + if (k->n_crossings < 8) { + k->crossings[k->n_crossings][0] = x; + k->crossings[k->n_crossings][1] = y; + k->crossings[k->n_crossings][2] = 0; + k->n_crossings++; + } + if (m->n_crossings < 8) { + m->crossings[m->n_crossings][0] = x; + m->crossings[m->n_crossings][1] = y; + m->crossings[m->n_crossings][2] = 0; + m->n_crossings++; + } + if (k->layer == -1) { + k->layer = *layer; + *layer = *layer + 1; + } + if (m->layer == -1) { + m->layer = *layer; + *layer = *layer + 1; + } + } + } +} + + /* + * Calculate the entry and exit angles of a band + * around a constraint. + */ + +void +calc_angles( + band_attrib_t *type, + LSL_band_constraint *k, + FILE *output) +{ + PRECISION first_x, first_y, last_x, last_y; + PRECISION dx,dy; + PRECISION angle,ta; + int i; + float n; + PRECISION pi = 2*atan2(1,0); + + if (k->cross || ! k->inside) { + first_x = k->end_angle[0]; + first_y = k->end_angle[1]; + last_x = k->start_angle[0]; + last_y = k->start_angle[1]; + } else { + first_x = k->start_angle[0]; + first_y = k->start_angle[1]; + last_x = k->end_angle[0]; + last_y = k->end_angle[1]; + } + + dx = (first_x - k->part.offset[0]); + dy = (first_y - k->part.offset[1]); + dx /= k->radius; + dy /= k->radius; + + // Warning!! acos() will give NAN if we give it badly normalized numbers. + if (dx > 1.0) + dx = 1.0; + if (dx < -1.0) + dx = -1.0; + + if (dy > 0) { + angle = acos(dx); + } else { + angle = -acos(dx); + } + +#define REORIENT_TREAD 1 +//#define SHOW_XY_PLANE_FOR_DEBUG 1 +#define DEBUGGING_FIXED3_BANDS 1 +// #define STRETCH_FIXED3 1 + +#define USE_TURN_ANGLE 1 +#ifdef USE_TURN_ANGLE + // The problem here is this: + // angle is the angle at which the tangent leaving this constraint starts at. + // I need to get THE DIFFERENCE between that angle and the previous constraint + // before I can calculate n_steps. + // See get_turn_mat() fn in curve.t for calculating the turn angle + + { + PRECISION r; + PRECISION a[3]; + PRECISION b[3]; + + extern PRECISION dotprod(PRECISION a[3], PRECISION b[3]); + + if (k->cross || ! k->inside) { + vectorcp(a, k->end_angle); + vectorcp(b, k->start_angle); + } else { + vectorcp(b, k->end_angle); + vectorcp(a, k->start_angle); + } + + vectorsub(a, k->part.offset); + vectorsub(b, k->part.offset); + normalize(a); + normalize(b); + + //Dot product gives turn angle. a.b=|a||b|cos(theta) + //We normalized so |a|=|b|=1, which means theta = acos(a.b). + r = dotprod(b, a); + // Warning!! acos() will give NAN if we give it badly normalized numbers. + if (r > 1.0) + r = 1.0; + if (r < -1.0) + r = -1.0; + + ta = r; + r = acos(r); + + // Check crossprod(b, a) to see if r is CW or CCW. + i = turn_vector(b, a); + + // Handle round off error near 0 degree turn + if ((ta + ACCY) > 1.0) // dotprod() is really 1 so call it no turn. + i = 0; + + if (i > 0) + r = 2*pi - r; + + else if (i == 0) // Handle 0 or 180 by the sign of dotprod() + { + if (ta < 0) + r = pi; + else if (k->layer == -2) // The ONLY constraint. + r = 2*pi; // Cover the whole thing. + else + r = 0; // Skip on past this constraint. + } + + if (k->layer == -2) + k->layer == -1; + +#ifdef DEBUGGING_FIXED3_BANDS + if (k->cross || ! k->inside) + printf("OUT(%.2fx, %.2fy, %dr) A = %.2f from (%.2f, %.2f) r = %.2f (%dT%.2f)\n", + k->part.offset[0], k->part.offset[1], (int)k->radius, + angle * 180 / pi, dx, dy, r * 180 / pi, i, ta); + else + printf("IN (%.2fx, %.2fy, %dr) A = %.2f from (%.2f, %.2f) r = %.2f (%dT%.2f)\n", + k->part.offset[0], k->part.offset[1], (int)k->radius, + angle * 180 / pi, dx, dy, r * 180 / pi, i, ta); +#endif + + ta = r; + } + +#endif + + k->s_angle = angle; + + // NOTE: Start converting FIXED3 to FIXED(N) with N segments. + // Probably should use something more like a convex hull for FIXED(N) + // Gotta review all this STRETCH_FIXED3 stuff. What was I thinking??? + // + // I should measure the length of the convex hull and stretch it to + // fit N segments if needed. (Maybe also compress it if a bit too big?) + // + // Remember FIXED3 was created to indicate we need 3 part types to describe + // a (probably rubbery) band (straights, arcs, and transitions). + // That still applies, but now we also want to know how many segments, + // so extend it to FIXED(N) like the FIXED(N) hoses. + // After all, when you think about it, you only need the special blending + // parts when the material is flexible but segmented, which almost implies + // rubber treads. However I suppose we could retain the special FIXED3 + // function just in case they make something in multiple sizes. + // We don't actually lose much even if we store FIXED3 as type->fill == 3 + // because a 3 sided rubber tread is pretty dull stuff. + // But storing it that way does simplify a bunch of code. + // + // Now, do we need full convex hull code or can we get by with some + // turn_vector() tests. We can borrow that fn from stub.c in ldglite. + // Are we always going CCW around the constraints? + + //printf("Band FillType = %d \n",type->fill); + // If (type->fill > FIXED) it contains the length (in segments) of the band. + + if ((type->fill == FIXED3) || (type->fill > FIXED)) { + PRECISION circ; + if (angle < 0) { + angle = - angle; + } +#ifdef USE_TURN_ANGLE + circ = ta*k->radius; +#else + circ = angle*2*k->radius; +#endif +#ifdef STRETCH_FIXED3 + // Do not round up the number of arc steps. Stretch the tangent lines instead. + k->n_steps = circ*type->scale; +#else + k->n_steps = circ*type->scale+0.5; +#endif + } else if (type->fill == FIXED) { + +#ifdef USE_TURN_ANGLE + PRECISION circ; + circ = ta*k->radius; + k->n_steps = circ*type->scale + 0.5; + k->n_steps++; // Not really steps, but segment endpoints? So add 1 more point. + printf("nsteps = %d = (%.2f / %.2f\n", k->n_steps, circ, 1.0 / type->scale); +#else + n = type->scale * 2 * pi * k->radius + 0.5; + + // circumference + for (i = 0; i < n; i++) { + PRECISION f; + f = i; + f /= n; + ta = angle - 2*pi*(1-f); + dx = k->radius*cos(ta) + k->part.offset[0] - last_x; + dy = k->radius*sin(ta) + k->part.offset[1] - last_y; + if (sqrt(dx*dx+dy*dy) < type->thresh) { + break; + } + } + k->n_steps = i+1; +#endif + } else { // (type->fill == STRETCH) + + n = 2 * pi * k->radius/band_res + 0.5; + + // circumference + for (i = 0; i < n; i++) { + PRECISION f; + f = i; + f /= n; + ta = angle - 2*pi*(1-f); + dx = k->radius*cos(ta) + k->part.offset[0] - last_x; + dy = k->radius*sin(ta) + k->part.offset[1] - last_y; + if (sqrt(dx*dx+dy*dy) < type->thresh) { + break; + } + } + k->n_steps = i+1; + + } +} + + /* + * figure out the intersections of a line and a circle. + * - line is in x = xo + f*t, y = yo + g*t (normalized parametric) form + * - circle is (xj, yj, rj) + */ + +int intersect_line_circle_2D( + PRECISION xo, + PRECISION yo, + PRECISION f, + PRECISION g, + PRECISION xj, + PRECISION yj, + PRECISION rj, + PRECISION *x, + PRECISION *y) +{ + PRECISION fsq, gsq, fgsq; + PRECISION xjo, yjo; + PRECISION fygx; + PRECISION fxgy; + PRECISION root; + PRECISION t; + + fsq = f * f; + gsq = g * g; + fgsq = fsq + gsq; // dx2 + dy2 (should be 1 if normalized). + + if (fgsq < ACCY) { + printf("line coefficients are corrupt\n"); // Because dx = dy = 0. + } + + xjo = xj - xo; + yjo = yj - yo; + fygx = f*yjo - g*xjo; + root = rj*rj*fgsq - fygx*fygx; + + // What's the story with this message? I get it sometimes but this still works. + // We eliminate the two degenerate cases (circles overlap) in calc_tangent_line() + // So this fn should ALWAYS work when it's called. I suspect this is bogus... + if (root < -ACCY) { + printf("line does not intersect with circle\n"); + } + + fxgy = f*xjo + g*yjo; + + t = fxgy/fgsq; + + *x = xo + f*t; + *y = yo + g*t; + return 0; +} + + /* + * determine the tangent we want. + */ + +int calc_tangent_line( + LSL_band_constraint *k, + LSL_band_constraint *l, + FILE *output) +{ + int inside1, inside2; + PRECISION rl,rk,rlk; + PRECISION xlk,xlksq,ylk,ylksq; + PRECISION denom; + PRECISION radius; + PRECISION angle; + PRECISION rx,ry; + + inside1 = k->inside; + inside2 = l->inside; + + if (l->was_cross) { + if (l->cross) { + inside2 ^= 1; + } else { + inside1 ^= 1; + } + } + + rl = l->radius; + rk = k->radius; + + switch ((inside1 << 1) | inside2) { + case 3: /* inside to inside */ + /* end angle for i , start for j */ + /* start point line of ij line stored in j */ + /* endpoint of line of ij line stored in j */ + break; + case 2: /* inside to outside */ + rl = - rl; + break; + case 1: /* outside to inside */ + rk = -rk; + break; + case 0: /* outside to outside */ + rk = -rk; + rl = -rl; + break; + } + + rlk = rl - rk; + + xlk = l->part.offset[0] - k->part.offset[0]; + ylk = l->part.offset[1] - k->part.offset[1]; + + xlksq = xlk*xlk; + ylksq = ylk*ylk; + + denom = xlksq + ylksq; + + if (denom < ACCY) { + /* circles are coincident - badness */ + } else { + PRECISION root; + + root = denom - rlk*rlk; + if (root < -ACCY) { + /* tangent doesn exist */ + // ie. sqr(dist) < sqr(radius) + // NOTE: actually we should check for (root < +ACCY) because: + // For the outer tangent case this means one circle is completely + // inscribed within the other. + // For the inner tangent case this means the circles touch or + // overlap. + } else { + + PRECISION a,b,c; + PRECISION deninv,factor; + PRECISION xo,yo; + PRECISION f,g; + + if (root < 0) { + root = 0; + } + root = sqrt(root); + deninv = 1.0/denom; + a = (-rlk*xlk - ylk*root)*deninv; + b = (-rlk*ylk + xlk*root)*deninv; + c = -(rk + a*k->part.offset[0] + b*k->part.offset[1]); + + /* we have the normalized form of the tangent line */ + + /* now we map the line to parametric form */ + root = 1.0/(a * a + b * b); + factor = -c*root; + xo = a*factor; + yo = b*factor; + + root = sqrt(root); + + f = b*root; + g = -a*root; + + /* now line is in x = xo + f*t, y = yo + g*t form */ + /* calculate endpoints of each line */ + + intersect_line_circle_2D( + xo, + yo, + f, + g, + k->part.offset[0], + k->part.offset[1], + k->radius, + &k->start_line[0], + &k->start_line[1]); + k->start_line[2] = 0; + vectorcp(k->start_angle,k->start_line); + + intersect_line_circle_2D( + xo, + yo, + f, + g, + l->part.offset[0], + l->part.offset[1], + l->radius, + &k->end_line[0], + &k->end_line[1]); + k->end_line[2] = 0; + vectorcp(l->end_angle,k->end_line); + + // this means we need our previous neighbor's end line and our + // start line to know the arc + } + } + return 0; +} + + /* + * Render the arc around the constraint and the line to the next + * constraint + */ + +int draw_arc_line( + band_attrib_t *type, + LSL_band_constraint *constraint, + int color, + int draw_line, + FILE *output, + int ghost, + char *group, + part_t *absolute, + LSL_band_constraint *f_constraint) +{ + int i,j,n; + PRECISION dx,dy,dz; + PRECISION L1,L2; + PRECISION inv[3][3]; + int steps; + + if (draw_line) { + + for (j = 0; j < constraint->n_crossings - 1; j++) { + PRECISION orient[3][3]; + + // determine the orientation of the part in the XY plane + + dx = constraint->crossings[j+1][0] - constraint->crossings[j][0]; + dy = constraint->crossings[j+1][1] - constraint->crossings[j][1]; + dz = constraint->crossings[j+1][2] - constraint->crossings[j][2]; + + L1 = sqrt(dx*dx + dy*dy); + L2 = sqrt(dx*dx + dy*dy + dz*dz); +#ifdef DEBUGGING_FIXED3_BANDS + printf("Direction = (%.3f, %.3f, %.3f) => %.3f_L1, %.3f_L2)\n", dx, dy, dz, L1, L2); + // ****************************** + // Based on this it looks like L2 is already in whole number units of part len + // (where part len is 1/type->scale). + // In order to be able to stretch things a bit to fit, I need a raw length + // in L1 or L2. (L2 should always = L1 since dz=0 when we move to XY plane.) + // + // Maybe not. It just works out that way here in my example. + // Really what I need to do is delay the line drawing until after the arcs. + // Calculate the arcs (making sure to always round n arc segs down) + // Then move the j+1 crossings to meet the ends of the arcs, + // stretching L1 a bit in the process. +#endif + if (L1 == 0) { + + orient[0][0] = 1; + orient[1][0] = 0; + orient[2][0] = 0; + orient[0][1] = 0; + orient[1][1] = 0; + orient[2][1] =-1; + orient[0][2] = 0; + orient[1][2] = 1; + orient[2][2] = 0; + } else { + orient[0][0] = dy/L1; // cos + orient[1][0] = -dx/L1; // sin + orient[2][0] = 0; + orient[0][1] = dx/L2; // -sin + orient[1][1] = dy/L2; // cos + orient[2][1] = dz/L2; + orient[0][2] = -dx*dz/(L1*L2); + orient[1][2] = -dy*dz/(L1*L2); + orient[2][2] = L1/L2; + } + + if (type->fill == STRETCH) { + n = 1; + steps = 0; + } else if (type->fill == FIXED3) { + n = L2*type->scale+0.5; + steps = 1; + } else if (type->fill > FIXED) { + n = L2*type->scale+0.5; + steps = 1; + } else { // FIXED + n = L2*type->scale+0.5; + steps = 0; + } + +#ifdef STRETCH_FIXED3 + L1 = 1; +#endif + for (i = steps; i < n; i++) { + part_t part; + PRECISION tm[3][3]; + PRECISION foffset[3]; + + part = type->tangent; + + if (type->fill == STRETCH) { + PRECISION scale[3][3]; + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + scale[i][j] = 0; + } + } + scale[0][0] = 1; + scale[1][1] = L2; + scale[2][2] = 1; + matrixmult(part.orient,scale); + } +#ifdef STRETCH_FIXED3 + else if ((type->fill == FIXED3) || (type->fill > FIXED)) { + PRECISION scale[3][3]; + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + scale[i][j] = 0; + } + } + scale[0][0] = 1; + scale[1][1] = 1; + scale[2][2] = 1; + if (n > 1) + scale[0][0] = L1 = (L2*type->scale -1)/(n-1); + matrixmult(part.orient,scale); + } + +#endif +#ifdef DEBUGGING_FIXED3_BANDS + if (i == steps) + printf("Scale(%.2f, %.3f, %d) = %.2f)\n", L2, type->scale, n, L1); +#endif + + // We performed this to start: + // 1. move the assembly so the first constraint is at the origin + // 2. rotate offset ations about the inverse of the first constraint's + // orientation bringing everything into the X/Y plane + + // Now we're putting together the segments and putting them back into place + // 1. multiply tangent part orientation times the orient + // so we know how to orient the tangent part in the X/Y plane. + // 2. rotate the tangent offsets into the X/Y plane. + + matrixcp(tm,part.orient); + matrixmult3(part.orient,orient,tm); + + vectorrot3(part.offset,type->tangent.offset,part.orient); + + // 3. Calculate the tangent part's orientation in the X/Y plane + + foffset[0] = constraint->crossings[j][0] + dx * i / n - part.offset[0]; + foffset[1] = constraint->crossings[j][1] + dy * i / n - part.offset[1]; + foffset[2] = constraint->crossings[j][2] + dz * i / n - part.offset[2]; + +#ifdef STRETCH_FIXED3 + foffset[0] += (L1-1) * dx * (i-1) / ((PRECISION)n -0.5); + foffset[1] += (L1-1) * dy * (i-1) / ((PRECISION)n -0.5); + foffset[2] += (L1-1) * dz * (i-1) / ((PRECISION)n -0.5); +#endif + + vectorcp(part.offset,foffset); + +#ifdef REORIENT_TREAD + // 4. Reorient the tangent part in the X/Y plane based on the first + // constraint's offset (for things like technic turntable top, + // where the gear plane does not go through the origin. + + vectorsub( part.offset,band_constraints[f_constraint->band_constraint_n].offset); + + // 5. Orient tangent part based on the orientation of the first + // constraint's orientation (for things like technic turntable + // who's gear plane is perpendicular to the plane of say the 24T + // gears. + + matrixinv(inv,band_constraints[f_constraint->band_constraint_n].orient); + vectorrot( part.offset,inv); + + // 6. Orient the tangent part offsetation back to the absolute 3D + // offsetation. + + vectorrot( part.offset,absolute->orient); + + // 7. Now add the absolute orientation of the first constraint + + vectoradd( part.offset,absolute->offset); + + // 8. Change the tangent part orientation based on the first + // constraint's orientation (again for things like the + // technic turntable top, where the gear plane is perpendicular + // to the standard 24T gear's gear plane + + matrixcp(tm,part.orient); + matrixmult3(part.orient, inv, tm); + + // 9. Now rotate the part back into its correct orientation in 3D + // space. + + matrixcp(tm,part.orient); + matrixmult3(part.orient,absolute->orient,tm); +#endif // REORIENT_TREAD + + output_line( + output, + ghost, + group, + color, + part.offset[0],part.offset[1],part.offset[2], + part.orient[0][0],part.orient[0][1],part.orient[0][2], + part.orient[1][0],part.orient[1][1],part.orient[1][2], + part.orient[2][0],part.orient[2][1],part.orient[2][2], + type->tangent.type); + } + } + } + + // Create the arc + + { + PRECISION pi = 2*atan2(1,0); + PRECISION f[3]; + + /* now for the arc */ + + if (type->fill == STRETCH) { + n = 2*pi*constraint->radius/band_res; + } else { + n = 2*pi*constraint->radius*type->scale; + } + + // vector for the first arc part + + f[0] = constraint->radius * cos(constraint->s_angle); + f[1] = constraint->radius * sin(constraint->s_angle); + f[2] = 0; + + if ((type->fill == FIXED3) || (type->fill > FIXED)) { + steps = constraint->n_steps + 2; + } else { + steps = constraint->n_steps; + } + + for (i = 1; i < steps; i++) { + PRECISION orient[3][3]; + PRECISION foffset[3]; + PRECISION tm[3][3]; + PRECISION s[3]; + part_t part; + + // for FIXED3 (e.g. rubber tread), the first and last segments are + // treated as transition pieces, otherwise just use arc parts + +#ifdef NEW_BAND_CODE_BUT_STILL_NEEDS_WORK + if (((type->fill == FIXED3) || (type->fill > FIXED)) + && i == 1 && i+1 == steps) { + part = type->tangent; // Only one part. It probably should be straight. + // Of course the right way would be to check the turn angle here, + // but we don't save it (r) from calc_angles(). + } else +#endif + if (((type->fill == FIXED3) || (type->fill > FIXED)) && i == 1) { + part = type->start_trans; + } if (((type->fill == FIXED3) || (type->fill > FIXED)) && i+1 == steps) { + part = type->end_trans; + } else { + part = type->arc; + } + + // rotate the arc part so it hugs the current constraint + + s[0] = constraint->radius * cos(constraint->s_angle + 2*pi*i/n); + s[1] = constraint->radius * sin(constraint->s_angle + 2*pi*i/n); + s[2] = 0; + + dx = s[0] - f[0]; + dy = s[1] - f[1]; + dz = s[2] - f[2]; + + L1 = sqrt(dx*dx + dy*dy); + L2 = sqrt(dx*dx + dy*dy + dz*dz); + + if (L1 == 0) { + orient[0][0] = 1; + orient[1][0] = 0; + orient[2][0] = 0; + orient[0][1] = 0; + orient[1][1] = 0; + orient[2][1] = -1; + orient[0][2] = 0; + orient[1][2] = 1; + orient[2][2] = 0; + } else { + orient[0][0] = dy/L1; + orient[1][0] = -dx/L1; + orient[2][0] = 0; + orient[0][1] = dx/L2; + orient[1][1] = dy/L2; + orient[2][1] = dz/L2; + orient[0][2] = -dx*dz/(L1*L2); + orient[1][2] = -dy*dz/(L1*L2); + orient[2][2] = L1/L2; + } + + if (type->fill == STRETCH) { + PRECISION scale[3][3]; + PRECISION angle = 2 * pi / n; + PRECISION l = type->scale*sin(angle); + int k,j; + + if (i + 1 == steps) { + l *= 5; + } + + for (k = 0; k < 3; k++) { + for (j = 0; j < 3; j++) { + scale[k][j] = 0; + } + } + scale[0][0] = 1; + scale[1][1] = L2+l; + scale[2][2] = 1; + matrixmult(part.orient,scale); + } + + // Now we're putting together the segments and putting them back into place + // 1. multiply arc part orientation times the orient + // so we know how to orient the arc part in the X/Y plane. + // 2. rotate the arc offsets into the X/Y plane. + + matrixcp(tm,part.orient); + matrixmult3(part.orient,orient,tm); + +#ifdef NEW_BAND_CODE_BUT_STILL_NEEDS_WORK + if (((type->fill == FIXED3) || (type->fill > FIXED)) + && i == 1 && i+1 == steps) // if (part == type->tangent) + vectorrot3(part.offset,type->tangent.offset,part.orient); + else +#endif + vectorrot3(part.offset,type->arc.offset,part.orient); + + // 3. Calculate the arc part's offsetation in the X/Y plane + + foffset[0] = f[0] + constraint->part.offset[0] - part.offset[0]; + foffset[1] = f[1] + constraint->part.offset[1] - part.offset[1]; +#ifdef WTF_IS_THIS + foffset[2] = f[2] + constraint->part.offset[2] - part.offset[2]; +#else + // I thought the constraint Z coordinate was always zero in the X/Y plane. + foffset[2] = f[2] + 0 - part.offset[2]; +#endif + + vectorcp(part.offset,foffset); + +#ifdef REORIENT_TREAD + // 4. Rotate the arc part in the X/Y plane based on the first + // constraint's offset (for things like technic turntable top, + // where the gear plane does not go through the origin.) + + vectorsub(part.offset,band_constraints[f_constraint->band_constraint_n].offset); + + // 5. Orient arc part based on the orientation of the first + // constraint's orientation (for things like technic turntable + // who's gear plane is perpendicular to the plane of say the 24T + // gears.) + + matrixinv(inv,band_constraints[f_constraint->band_constraint_n].orient); + vectorrot(part.offset,inv); + + // 6. Orient the arc part offsetation back to the absolute 3D + // offsetation. + + vectorrot(part.offset,absolute->orient); + + // 7. Now add the absolute offsetation of the first constraint + + vectoradd( part.offset,absolute->offset); + + // 8. Change the arc part orientation based on the first + // constraint's orientation (again for things like the + // technic turntable top, where the gear plane is perpendicular + // to the standard 24T gear's gear plane) + + matrixcp(tm,part.orient); + matrixmult3(part.orient, inv, tm); + + // 9. Now rotate the part back into its correct orientation in 3D + // space. + + matrixcp(tm,part.orient); + matrixmult3(part.orient,absolute->orient,tm); +#endif // REORIENT_TREAD + + output_line( + output, + ghost, + group, + color, + part.offset[0],part.offset[1],part.offset[2], + part.orient[0][0],part.orient[0][1],part.orient[0][2], + part.orient[1][0],part.orient[1][1],part.orient[1][2], + part.orient[2][0],part.orient[2][1],part.orient[2][2], + part.type); + + vectorcp(f,s); + } + } + + return 0; +} + + void +showconstraints( + FILE *output, + LSL_band_constraint *constraints, + int n_constraints, + int color) +{ +#if 1 + int i; + + for (i = 0; i < n_constraints; i++) { + part_t *cp = &constraints[i].part; + output_line( + output, + 0, + NULL, + color, + cp->offset[0], cp->offset[1], cp->offset[2], + cp->orient[0][0],cp->orient[0][1],cp->orient[0][2], + cp->orient[1][0],cp->orient[1][1],cp->orient[1][2], + cp->orient[2][0],cp->orient[2][1],cp->orient[2][2], + cp->type); + } + + fflush(output); +#endif +} + +static void +rotate_constraints( + LSL_band_constraint *constraints, + int n_constraints, + PRECISION m[3][3]) +{ + int i; + PRECISION t[3][3]; + + for (i = 0; i < n_constraints-1; i++) { + vectorrot(constraints[i].part.offset,m); + matrixcp(t,constraints[i].part.orient); + matrixmult3(constraints[i].part.orient,m,t); + } +} + + /* + * This subroutine synthesizes planar rubber bands, chain, and treads. + * + * We do all the arc and tangent analysis in the X/Y plane. + * + * The synthesis plane is defined by the first constraint. First we move the + * first constaint to the origin. Then we calculate the inverse of the first + * constraints orientation, and multiply all the constraints' offsetations by + * the inverse of the first constraint's orientation. + * + * Some of the gears' are oriented in the X/Y plane, while other gears are + * oriented in the X/Z plane. The band_constraints array above, describes each of the + * supported constraint types, and their orientation. We multiply all the + * constraints by the first constraint's band_constraints orientation. + * + * Also, in some cases, some of the constraint types described in band_constraints + * need to be offset to get the place where the band should hit onto the + * X/Y plane. + */ + +//***************************************************************** +// NOTES: +// +// What's the meaning of the tangent directives? (INSIDE, OUTSIDE, CROSS) +// INSIDE, OUTSIDE refer to where we want the next wheel to be. +// It's either gonna be placed inside the band, or outside the band. +// If I move the constraints in the XY plane and draw the band CCW +// then the next wheel will be left of the band for INSIDE. +// The constraint will be to the right of the band for OUTSIDE. +// The default is INSIDE. +// CROSS means OUTSIDE, then INSIDE. It can be used to make an X. +// +// CROSS really only should be allowed for string and rubber bands. +// It doesn't make much sense for rubber treads, although I suspect +// it could be used to put one wheel of three on the outside. +// +//***************************************************************** + +int +synth_band( + char *type, + int n_constraints, + LSL_band_constraint *constraints, + int color, + FILE *output, + int ghost, + char *group) +{ + int i; + int cross = 0; + int was_cross = 0; + int inside = 1; + int first,last; + part_t absolute; + PRECISION inv[3][3],trot[3][3]; + int layer = 0; + + // Make band_type a global var so we can use it everywhere. + // band_attrib_t *band_type = NULL; + + /* Search for band type */ + band_type = NULL; + for (i = 0; i < N_BAND_TYPES; i++) { + if (strcasecmp(type,band_types[i].type) == 0) { + band_type = &band_types[i]; + break; + } + } + if (band_type == NULL) { + return 0; + } + + // Hmmm, maybe I should redirect all info messages to stderr + // (so lsynth can be used as a filter). + // Or maybe redirect to stderr ONLY if its a filter (if (output == stdout)) + + if (n_constraints < 1) { + fprintf(stderr, "No BAND constraints found.\n"); + return 0; + } + + first = -1; + + for (i = 0; i < n_constraints; i++) { + constraints[i].radius = 0; + constraints[i].inside = inside; + constraints[i].cross = cross; + constraints[i].was_cross = was_cross; + constraints[i].n_crossings = 0; + constraints[i].layer = -1; + was_cross = 0; + if (strcasecmp(constraints[i].part.type,"INSIDE") == 0) { + inside = 1; + } else if (strcasecmp(constraints[i].part.type,"OUTSIDE") == 0) { + inside = 0; + } else if (strcasecmp(constraints[i].part.type,"CROSS") == 0) { + inside ^= 1; + + } else { + int k; + + // search the constraints table + + for (k = 0; k < N_BAND_CONSTRAINTS; k++) { + if (strcasecmp(constraints[i].part.type,band_constraints[k].type) == 0) { + constraints[i].band_constraint_n = k; + + constraints[i].radius = band_constraints[k].attrib; + break; + } + } + } + if (first == -1 && constraints[i].radius) { + first = i; + } + } + +#ifdef USE_TURN_ANGLE + if (n_constraints == 1) { + constraints[first].layer = -2; // Mark this as the ONLY constraint. + } +#endif + + /* create an extra constraint that represents the final state of the + * first pulley */ + if (band_type->pulley == 0) { + memcpy(&constraints[i],&constraints[first],sizeof(constraints[i])); + n_constraints++; + } + constraints[i].inside = inside; + constraints[i].cross = cross; + constraints[i].was_cross = was_cross; + + /* record the first constraint in its original form */ + + absolute = constraints[first].part; + + /* 1. move the first constraint to the origin */ + + for (i = 0; i < n_constraints; i++) { + if (constraints[i].radius) { + vectorsub(constraints[i].part.offset,absolute.offset); + } + } + + // showconstraints(output,constraints,n_constraints,14); + + /* 2. bring the entire assembly into the part's natural orientation */ + + matrixinv(inv,absolute.orient); + + rotate_constraints(constraints,n_constraints,inv); + + // showconstraints(output,constraints,n_constraints,4); + + /* 3. bring the assembly into the X/Y plane (necessary for first constraints + * who's gear plane is different that the default gear plane used by + * simple gears, like technic turntable). + */ + + rotate_constraints(constraints,n_constraints, + band_constraints[constraints[first].band_constraint_n].orient); + + //showconstraints(output,constraints,n_constraints,15); + + /* 4. Now that the whole assembly is in AN the X/Y plane, move everything + * so the center of the axle of the first constraint is at the origin. + * With the current list of band constraints that means maybe adjusting + * the Z Coordinates. + */ + + for (i = 0; i < n_constraints; i++) { + if (constraints[i].radius) { + vectoradd(constraints[i].part.offset, //was vectorsub() + band_constraints[constraints[first].band_constraint_n].offset); + } + } + +#if 0 + /* 5. Offset constraints that were not modeled at the orgin (or on an axis). + * + * At this point we can assume all constraint centers are in THE X/Y plane + * at Z=0. However the part origins may need to be moved in X and/or Y + * if for some reason the part was not modeled symmetrically about either + * the X, Y, or Z axis. Do not change the Z at this point. + * And skip the first constraint because we already did it above (step 4). + * + * Do not undo this step in the final reorient code. It's one way. + */ + + for (i = 1; i < n_constraints; i++) { // Start at constraint 1, not 0. + if (constraints[i].radius) { + vectoradd(constraints[i].part.offset, + band_constraints[constraints[i].band_constraint_n].offset); + constraints[i].part.offset[2] = 0; // Set Z to zero, just in case. + } + } +#endif + + //*************************************************************************** + //NOTE: This dies if I use only constraints NOT listed as band constraints. + // By the time I get here constraints[0].part->type is a mangled string. + // Perhaps the whole constraint part is crap, or perhaps just the name. + //*************************************************************************** + + fflush(output); +#ifdef DEBUGGING_FIXED3_BANDS + for (i = 0; i < n_constraints; i++) { + if (constraints[i].radius) { + printf("constraint[%d] = (%d, %d, %d)\n", i, + (int)constraints[i].part.offset[0], + (int)constraints[i].part.offset[1], + (int)constraints[i].part.offset[2] ); + } + } +#endif +#ifdef SHOW_XY_PLANE_FOR_DEBUG + showconstraints(output,constraints,n_constraints,3); +#endif + + /* figure out the tangents' intersections with circles */ + + first = -1; + last = -1; + + for (i = 0; i < n_constraints - 1; ) { + if (constraints[i].radius) { + int j; + if (first == -1) { + first = i; + } + for (j = i+1; j < n_constraints; j++) { + if (constraints[j].radius) { +#ifdef DEBUGGING_FIXED3_BANDS + printf("calc_tan(%d->%d)\n", i, j); +#endif + calc_tangent_line(&constraints[i],&constraints[j],output); + i = j; + last = j; + break; + } + } + if (j == n_constraints+1) { + i++; + } + } else { + i++; + } + } + vectorcp(constraints[first].end_angle,constraints[last].end_angle); + + /* calculate intersections between band straight line segments, so + * we can make the line segments go around each other. + */ + + for (i = 0; i < n_constraints; i++) { + if (constraints[i].radius) { + vectorcp(constraints[i].crossings[0],constraints[i].start_line); + constraints[i].n_crossings = 1; + } + } + for (i = 0; i < n_constraints- 1; i++) { + if (constraints[i].radius) { + int j; + for (j = i+1; j < n_constraints; j++) { + if (constraints[j].radius) { + calc_crosses(&constraints[i],&constraints[j],&layer,output); + } + } + } + } + + +#ifdef SHOW_XY_PLANE_FOR_DEBUG + for (i = 0; i < n_constraints-1; i++) { + part_t *cp = &constraints[i].part; + LSL_band_constraint *k = &constraints[i]; + int color = 4; + output_line( + output, + 0, + NULL, + color, + //cp->offset[0]+k->start_line[0],cp-> offset[1]+k->start_line[0], cp->offset[2]+k->start_line[2], + k->start_line[0], k->start_line[1], k->start_line[2], + cp->orient[0][0],cp->orient[0][1],cp->orient[0][2], + cp->orient[1][0],cp->orient[1][1],cp->orient[1][2], + cp->orient[2][0],cp->orient[2][1],cp->orient[2][2], + "LS02.dat"); + color = 2; + output_line( + output, + 0, + NULL, + color, + //cp->offset[0]+k->start_line[0],cp-> offset[1]+k->start_line[0], cp->offset[2]+k->start_line[2], + k->end_line[0], k->end_line[1], k->end_line[2], + cp->orient[0][0],cp->orient[0][1],cp->orient[0][2], + cp->orient[1][0],cp->orient[1][1],cp->orient[1][2], + cp->orient[2][0],cp->orient[2][1],cp->orient[2][2], + "LS02.dat"); + } +#endif + + +#define BAND_DIAM 4 + + /* calculate the depth for each band at crossing */ + + if (layer > 0) { + { + PRECISION layer_offset; + layer_offset = (((layer) / 2) % 2) * BAND_DIAM/2; + //layer_offset = 0; + for (i = 0; i < n_constraints-1; i++) { + if (constraints[i].radius) { + int j; + for (j = 1; j < constraints[i].n_crossings; j++) { + constraints[i].crossings[j][2] = + (constraints[i].layer-layer/2)*BAND_DIAM + layer_offset; + } + } + } + } + } + + for (i = 0; i < n_constraints-1; i++) { + if (constraints[i].radius) { + vectorcp(constraints[i].crossings[constraints[i].n_crossings++], + constraints[i].end_line); + } + } + + for (i = 0; i < n_constraints-1; i++) { + if (constraints[i].radius) { + calc_angles(band_type,&constraints[i],output); + } + } + + /***************************************************** + * rotate everything back to whence it came + * and put all parts back to original absolute + * coordinates. + *****************************************************/ + + if ( ! ldraw_part) { + fprintf(output,"0 SYNTH SYNTHESIZED BEGIN\n"); + } + + group_size = 0; + + /* now draw out the rubber band in terms of lines and arcs */ + for (i = 0; i < n_constraints; ) { + if (constraints[i].radius != 0) { + int j; + for (j = i+1; j < n_constraints; j++) { + + if (constraints[j].radius) { + draw_arc_line( + band_type, + &constraints[i], + color, + n_constraints > 1, + output, + ghost, + group, + &absolute, + &constraints[first]); + i = j; + break; + } + } + if (j >= n_constraints) { + i++; + } + } else { + i++; + } + } + if (group) { + fprintf(output,"0 GROUP %d %s\n",group_size,group); + } + if ( ! ldraw_part) { + fprintf(output,"0 SYNTH SYNTHESIZED END\n"); + } + return 0; + +} + diff --git a/lsynth/src/band.h b/lsynth/src/band.h new file mode 100644 index 000000000..7bb19440f --- /dev/null +++ b/lsynth/src/band.h @@ -0,0 +1,73 @@ +/* + * This file describes the interface to the LDRAW synthesizable parts library. + * Kevin Clague + */ +#ifndef BAND_H +#define BAND_H + +#ifdef _cplusplus +extern "C" { +#endif + +typedef struct { + part_t part; + PRECISION radius; + int inside; + int was_cross; + int cross; + PRECISION start_line[3]; + PRECISION end_line[3]; + PRECISION crossings[8][3]; + int n_crossings; + int layer; + + PRECISION start_angle[3]; + PRECISION end_angle[3]; + int n_steps; + PRECISION s_angle; + + int band_constraint_n; +} LSL_band_constraint; + +typedef struct { + char type[256]; // name of thing being specified (e.g. RUBBER_BAND) + char descr[256]; + int fill; // method for synthesizing + // FIXED - chain and tread are composed of fixed + // length parts + // FIXED3 - special case for rubber tread. + // STRETCH - for rubber bands + PRECISION scale; // convert LDUs to number of parts + PRECISION thresh; // used to compute number of steps on arc + int pulley; // initially for STRING on pulleys + part_t tangent; // the part type used for tangent + part_t arc; // the part type used for going around constraints + part_t start_trans; // for rubber treads, transition from arc to tangent + part_t end_trans; // for rubber treads, transition from tangent to arc +} band_attrib_t; + +extern band_attrib_t band_types[]; +extern int n_band_types; +extern part_t band_constraints[]; +extern int n_band_constraints; + +void list_band_types(void); +void band_ini( void); +int isbandtype( char *type); +int isbandconstraint(char *type); + +int +synth_band( + char *type, + int n_constraints, + LSL_band_constraint *constraints, + int color, + FILE *output, + int ghost, + char *group); + +#ifdef _cplusplus +}; +#endif + +#endif diff --git a/lsynth/src/curve.c b/lsynth/src/curve.c new file mode 100644 index 000000000..6425c77f5 --- /dev/null +++ b/lsynth/src/curve.c @@ -0,0 +1,1038 @@ +/* + * This is the LDRAW parts synthesis library. + * By Kevin Clague + */ + +#include "lsynthcp.h" +#include "hose.h" +#include "curve.h" +#include "mathlib.h" + +// Find the cross product. Also return the magnitude for error checking. +static PRECISION +mult_point(PRECISION r[3], PRECISION lhs[3], PRECISION rhs[3]) +{ + PRECISION tt; + + r[0] = lhs[1]*rhs[2] - lhs[2]*rhs[1]; + r[1] = lhs[2]*rhs[0] - lhs[0]*rhs[2]; + r[2] = lhs[0]*rhs[1] - lhs[1]*rhs[0]; + + tt = r[0]*r[0] + r[1]*r[1] + r[2]*r[2]; + + if (tt > 0.0) + { + tt = sqrt(tt); + + r[0] /= tt; + r[1] /= tt; + r[2] /= tt; + } + return tt; +} + +static void +make_rotation_pp( + PRECISION r[3][3], + PRECISION up[3], + PRECISION front[3]) +{ + PRECISION side[3]; + + mult_point(side,front,up); + + r[0][0] = up[0]; + r[1][0] = up[1]; + r[2][0] = up[2]; + r[0][1] = front[0]; + r[1][1] = front[1]; + r[2][1] = front[2]; + r[0][2] = side[0]; + r[1][2] = side[1]; + r[2][2] = side[2]; +} + +static void +rotate_point( + PRECISION r[3], + PRECISION m[3][3]) +{ + PRECISION t[3]; + + t[0] = r[0]*m[0][0] + r[1]*m[0][1] + r[2]*m[0][2]; + t[1] = r[0]*m[1][0] + r[1]*m[1][1] + r[2]*m[1][2]; + t[2] = r[0]*m[2][0] + r[1]*m[2][1] + r[2]*m[2][2]; + + r[0] = t[0]; + r[1] = t[1]; + r[2] = t[2]; +} + +void +orient2( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION up[3]; + int i; + + up[0] = 1; + up[1] = 0; + up[2] = 0; + + rotate_point(up,start->orient); + printf("%5.2f %5.2f %5.2f\n",up[0],up[1],up[2]); + + for (i = 0; i < n_segments-1; i++) { + PRECISION r; + PRECISION front[3]; + PRECISION t[3]; + + front[0] = segments[i+1].offset[0] - segments[i].offset[0]; + front[1] = segments[i+1].offset[1] - segments[i].offset[1]; + front[2] = segments[i+1].offset[2] - segments[i].offset[2]; + + r = sqrt(front[0]*front[0] + front[1]*front[1] + front[2]*front[2]); + + front[0] /= r; + front[1] /= r; + front[2] /= r; + + mult_point(t,front,up); // side + mult_point(up,t,front); // side * front + + make_rotation_pp(segments[i].orient,up,front); + } +} + +PRECISION hose_length( + int n_segments, + part_t *segments) +{ + PRECISION length = 0; + int i; + + for (i = 0; i < n_segments-1; i++) { + PRECISION l[3]; + + vectorsub3(l,segments[i+1].offset,segments[i].offset); + + length += vectorlen(l); + } + return length; +} + +PRECISION dotprod(PRECISION a[3], PRECISION b[3]); + +void +orient( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION start_up[3],end_up[3],up[3]; + PRECISION total_length = hose_length(n_segments,segments); + PRECISION cur_length; + int i; + + // The up vector of the constraint is along -Z so let's use that instead of +X. + // This way the resulting spin is predictable based on orient of the constraint. + start_up[0] = end_up[0] = 0; + start_up[1] = end_up[1] = 0; + start_up[2] = end_up[2] = -1; + rotate_point(start_up,start->orient); + rotate_point(end_up,end->orient); + +#ifdef DEBUG_ORIENT_MATH + // If the magnitude of the cross product is 0 then SE is colinear. + // Check dot product for direction. Negative dot product means 180. + // Positive dot product means 0 degree turn, skip up interpolation. + // If 180, we need to spin the up vector, not linearly interpolate. + // Because linear interpolation gives a zero magnitude up vector + // at the halfway point between start and end. + if (mult_point(up,start_up,end_up) == 0.0) + printf("Start_up X end = 0\n"); + else + printf("Start_up X end = good\n"); + cur_length = dotprod(start_up, end_up); + printf("dotprod(S,E) = %.2f\n", cur_length); +#endif + + /* Up vector controls the twist + * + * Interpolate the up vector based on start up vector, and + * end up vector, and how far we are down the hose's total + * length + */ + + for (i = 0; i < n_segments-1; i++) { + PRECISION r; + PRECISION front[3]; + PRECISION t[3]; + + cur_length = hose_length(i,segments); + + cur_length /= total_length; + + up[0] = start_up[0]*(1-cur_length) + end_up[0]*cur_length; + up[1] = start_up[1]*(1-cur_length) + end_up[1]*cur_length; + up[2] = start_up[2]*(1-cur_length) + end_up[2]*cur_length; + + r = sqrt(up[0]*up[0] + up[1]*up[1] + up[2]*up[2]); +#ifdef DEBUG_ORIENT_MATH + printf("UP length = %.3f\n", r); +#endif + + up[0] /= r; + up[1] /= r; + up[2] /= r; + + front[0] = segments[i+1].offset[0] - segments[i].offset[0]; + front[1] = segments[i+1].offset[1] - segments[i].offset[1]; + front[2] = segments[i+1].offset[2] - segments[i].offset[2]; + + r = sqrt(front[0]*front[0] + front[1]*front[1] + front[2]*front[2]); + + front[0] /= r; + front[1] /= r; + front[2] /= r; + +#ifdef DEBUG_ORIENT_MATH + r = mult_point(t,front,up); // side + printf("side axis = %.2f\n", r); + r = mult_point(up,t,front); // side * front + printf("up axis = %.2f\n", r); +#else + mult_point(t,front,up); // side + mult_point(up,t,front); // side * front +#endif + + make_rotation_pp(segments[i].orient,up,front); + } +} + +int +synth_curve( + part_t *start, + part_t *end, + part_t *segments, + int n_segments, + PRECISION attrib, + FILE *output) +{ + PRECISION vector[3]; + PRECISION start_speed_v[3]; + PRECISION stop_speed_v[3]; + PRECISION time,time2,i_time,i_time_sum,i_time_sum2,n_time; + PRECISION x,x2,y,y2,z,z2; + PRECISION ptp,ratio; + PRECISION ptp_sum; + int i,j,n; + +#if 0 + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x += start->orient[j][i] * vector[i]; + } + start_speed_v[j] = x; + } + + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x -= end->orient[j][i] * vector[i]; + } + stop_speed_v[j] = x; + } + + // stop_speed_v[2] = -stop_speed_v[2]; +#else + start_speed_v[0] = stop_speed_v[0] = 0; + start_speed_v[1] = attrib; + stop_speed_v[1] = -attrib; + start_speed_v[2] = stop_speed_v[2] = 0; + rotate_point(start_speed_v,start->orient); + rotate_point(stop_speed_v,end->orient); + + // Limit the stiffness factor to the distance between start and end + // constraints to avoid the curve doubling back upon itself. + // NOTE: This should only be done if the constraints point in the same + // general direction. ie. dotproduct > 0. + // (Use dotprod < 0 because -attrib reverses the direction of stop_speed.) + vectorsub3(vector, end->offset, start->offset); + x = vectorlen(vector); + y = dotprod(start_speed_v, stop_speed_v); + //printf("attrib = %.3f, dist = %.3f, dotprod = %.3f\n)",attrib, x, y); + if ((x < attrib*2) && (y <= 0.001)) + { + attrib = x/2.0; + //printf("newattrib = %.3f, dist = %.3f, dotprod = %.3f\n)",attrib, x, y); + start_speed_v[0] = stop_speed_v[0] = 0; + start_speed_v[1] = attrib; + stop_speed_v[1] = -attrib; + start_speed_v[2] = stop_speed_v[2] = 0; + rotate_point(start_speed_v,start->orient); + rotate_point(stop_speed_v,end->orient); + } +#endif + + for (i = 0; i < n_segments; i++) { + time = (PRECISION) i/ (PRECISION) n_segments; + + segments[i].offset[0] = + (1 - time) * (1 - time) * (1 - time) * start->offset[0] + + (1 - time) * (1 - time) * 3 * time * (start->offset[0] - start_speed_v[0]) + + (1 - time) * 3 * time * time * (end->offset[0] - stop_speed_v[0]) + + time * time * time * end->offset[0]; + + segments[i].offset[1] = + (1 - time) * (1 - time) * (1 - time) * start->offset[1] + + (1 - time) * (1 - time) * 3 * time * (start->offset[1] - start_speed_v[1]) + + (1 - time) * 3 * time * time * (end->offset[1] - stop_speed_v[1]) + + time * time * time * end->offset[1]; +/* +=(1-$A8)^3*D$4 + (1-$A8)^2*3*$A8*(D$4-D$3) + (1-$A8)*3*$A8^2*(D$5-D$6) + $A8^3*D$5 + */ + segments[i].offset[2] = + (1 - time) * (1 - time) * (1 - time) * start->offset[2] + + (1 - time) * (1 - time) * 3 * time * (start->offset[2] - start_speed_v[2]) + + (1 - time) * 3 * time * time * (end->offset[2] - stop_speed_v[2]) + + time * time * time * end->offset[2]; + } + + ptp_sum = 0; + + for (i = 0; i < n_segments - 1; i++) { + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + ptp_sum += ptp; + } + + /* G8 */ + + i_time_sum = 0; + for (i = 0; i < n_segments - 1; i++) { + time = (PRECISION) (i+1)/ (PRECISION) n_segments; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + + ratio = ptp*n_segments/ptp_sum; + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); + i_time_sum += i_time; + } + + /* H, I, J */ + n_time = 0; + i_time_sum2 = 0; + for (i = 0; i < n_segments - 1; i++) { + PRECISION foo; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x * x + y * y + z * z); /* E */ + ratio = ptp*n_segments/ptp_sum; /* F */ + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); /* G */ + i_time_sum2 += i_time; + + foo = 1.0/n_segments; + foo /= ratio; + foo /= i_time_sum; + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + n_time += foo; /* H */ + } + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + // orient(n_segments, segments); + + return 0; /* it all worked */ +} + + + + + +/***************************************************************/ +// Quaternion q[4] is mostly a direction vector with a spin(roll) angle. +// ie. quaternion(x,y,z,spin) +// +// Gotta compare quat FAQ conversions to ldglite conversions to +// see what to do with the weird ldraw coordinate system. I think I +// may have to reverse -y (and maybe z) to get right? handed system +// before using quaternions. Make sure I copied all conversions from +// same place to avoid mixed systems. + +/***************************************************************/ + +/***************************************************************/ +int normalizequat(PRECISION q[4]) +{ + PRECISION L; + + L = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + + if (L == 0.0) + return 0; // Uh Oh, the magnitude is zero. + + q[0] /= L; + q[1] /= L; + q[2] /= L; + q[3] /= L; + + return 1; +} + +/***************************************************************/ +int normalize(PRECISION v[3]) +{ + PRECISION L; + + L = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + + if (L == 0.0) + return 0; // Uh Oh, the magnitude is zero. + + v[0] /= L; + v[1] /= L; + v[2] /= L; + + return 1; +} + +/***************************************************************/ +/* returns det(m), m is 3x3 (3x4) matrix */ +/***************************************************************/ +PRECISION M3Det(PRECISION m[3][4]) /* Note argument type ! */ +{ + return (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) + + m[1][0] * (m[2][1] * m[0][2] - m[0][1] * m[2][2]) + + m[2][0] * (m[0][1] * m[1][2] - m[1][1] * m[0][2])); +} +/***************************************************************/ +// Convert axis v[3] and angle a to quaternion q[4]. +/***************************************************************/ +void axisangle2quat( + PRECISION v[3], + PRECISION a, + PRECISION q[4]) +{ + PRECISION sina,cosa; + + a = a / 2.0; + sina = sin(a); + cosa = cos(a); + q[0] = v[0] * sina; + q[1] = v[1] * sina; + q[2] = v[2] * sina; + q[3] = cosa; +} + +/***************************************************************/ +// Convert rotation matrix m[3][3] to quaternion q[4]. +// Beware! Quaternions cannot handle mirror matrices. +// This may be a problem because the ldraw coordinate system is mirrored? +/***************************************************************/ +void matrix2quat( + PRECISION m[3][3], + PRECISION q[4]) +{ + PRECISION T; + PRECISION S; + + // NOTE: First unmirror any mirrored matrix. + // (Multiplying the rotation matrix with the sign of its determinant). + PRECISION D = M3Det(m); + if (D < 0.0) + { + // TODO: Unmirror + } + +#ifdef SPEEDY_WAY +#define max(a,b) ((a > b) ? (a) : (b)) + + q[0] = sqrt( max( 0, 1 + m[0][0] - m[1][1] - m[2][2] ) ) / 2; + q[1] = sqrt( max( 0, 1 - m[0][0] + m[1][1] - m[2][2] ) ) / 2; + q[2] = sqrt( max( 0, 1 - m[0][0] - m[1][1] + m[2][2] ) ) / 2; + q[3] = sqrt( max( 0, 1 + m[0][0] + m[1][1] + m[2][2] ) ) / 2; + q[0] = _copysign( q[0], m[2][1] - m[1][2] ); + q[1] = _copysign( q[1], m[0][2] - m[2][0] ); + q[2] = _copysign( q[2], m[1][0] - m[0][1] ); + return; +#endif + + // Calculate the trace of the matrix T from the equation: + T = 1 + m[0][0] + m[1][1] + m[2][2]; + if ( T > 0.00000001 ) + { + S = sqrt(T) * 2; + q[0] = ( m[2][1] - m[1][2] ) / S; + q[1] = ( m[0][2] - m[2][0] ) / S; + q[2] = ( m[1][0] - m[0][1] ) / S; + q[3] = 0.25 * S; + } + // If the trace of the matrix is equal to zero then identify + // which major diagonal element has the greatest value. + // Depending on this, calculate the following: + else if ( (m[0][0] > m[1][1]) && (m[0][0] > m[2][2]) ) { // Column 0: + S = sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2] ) * 2; + q[0] = 0.25 * S; + q[1] = ( m[1][0] + m[0][1] ) / S; + q[2] = ( m[0][2] + m[2][0] ) / S; + q[3] = ( m[1][2] - m[2][1] ) / S; // q[3] = ( m[2][1] - m[1][2] ) / S; + } else if ( m[1][1] > m[2][2] ) { // Column 1: + S = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2; + q[0] = ( m[1][0] + m[0][1] ) / S; + q[1] = 0.25 * S; + q[2] = ( m[2][1] + m[1][2] ) / S; + q[3] = ( m[0][2] - m[2][0] ) / S; + } else { // Column 2: + S = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2; + q[0] = ( m[0][2] + m[2][0] ) / S; + q[1] = ( m[2][1] + m[1][2] ) / S; + q[2] = 0.25 * S; + q[3] = ( m[0][1] - m[1][0] ) / S; // q[3] = ( m[1][0] - m[0][1] ) / S; + } +} + +/***************************************************************/ +// Convert quaternion q[4] to rotation matrix m[3][3]. +/***************************************************************/ +void quat2matrix( + PRECISION q[4], + PRECISION m[3][3]) +{ + PRECISION a,b,c,s; + + normalizequat( q ); + + a = q[0]; + b = q[1]; + c = q[2]; + s = q[3]; + + m[0][0] = 1 - 2*b*b-2*c*c; + m[0][1] = 2*a*b - 2*s*c; + m[0][2] = 2*a*c + 2*s*b; + m[1][0] = 2*a*b+2*s*c; + m[1][1] = 1 - 2*a*a - 2*c*c; + m[1][2] = 2*b*c - 2*s*a; + m[2][0] = 2*a*c - 2*s*b; + m[2][1] = 2*b*c + 2*s*a; + m[2][2] = 1 - 2*a*a - 2*b*b; +} + +/***************************************************************/ +// Convert quaternion q[4] to a rotation axis v[3] and angle a. +/***************************************************************/ + +PRECISION quat2axisangle( + PRECISION q[4], + PRECISION v[3], + PRECISION *a) +{ + PRECISION cos_angle, sin_angle; + + normalizequat( q ); + + cos_angle = q[3]; + *a = acos( cos_angle ) * 2; + sin_angle = sqrt( 1.0 - cos_angle * cos_angle ); + + if ( fabs( sin_angle ) < 0.0005 ) + sin_angle = 1; + + v[0] = q[0] / sin_angle; + v[1] = q[1] / sin_angle; + v[2] = q[2] / sin_angle; + + return *a; +} + +//********************************************************************** +PRECISION dotprod(PRECISION a[3], PRECISION b[3]) +{ + return (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]); +} + +//********************************************************************** +// Calculate the the turn matrix M, turn axis T, and return the turn angle r. +// In case of emergency preload M and T with reasonable defaults. +// M is preloaded with previous M. +//********************************************************************** +PRECISION get_turn_mat(PRECISION M[3][3],PRECISION a[3],PRECISION b[3],PRECISION T[3]) +{ + PRECISION m[3][3]; + PRECISION q[4]; + PRECISION t[3]; + PRECISION r; + + //Dot product gives turn angle. a.b=|a||b|cos(theta) + //We normalized so |a|=|b|=1, which means theta = acos(a.b). + r = dotprod(b, a); + // Warning!! acos() will give NAN if we give it badly normalized numbers. + if (r > 1.0) + r = 1.0; + if (r < -1.0) + r = -1.0; + r = acos(r); + + //Cross product gives turn axis. + mult_point(t, a, b); + + // NOTE: gotta check cross product. + // If magnitude is zero we have either a 180 or 0 degree turn. + // + // Dot product can differentiate between 0 and 180 turn. + // Dot product of acute is negative (dot product of 180 = -1). + // Dot product of perpendicular is 0. + // Dot product of obtuse is positive (dot product of 0 = 1). + // + // If we get a 0 degree turn, just reuse previous orient matrix. + // If we get a 180 degree turn, reuse the previous turn axis. + // (Any axis in the perpendicular plane can make the 180, + // but we want to maintain the up vector, so reuse the previous axis.) + + // I think I can reasonably expect to avoid the 180 degree turns + // if we apply small incremental turns to the previous orient matrix + // instead of always applying the whole turn from the start matrix. + // But do we lose control of the up vector if we do that? + + if ((normalize(t) == 0) || (r == 0.0)) + { + if (dotprod(b, a) < 0) + { +#ifdef DEBUG_QUAT_MATH + printf("*** 180 degree turn!!!\n"); +#endif + // Just use the up vector (or previous t) which we preloaded into T. + vectorcp(t, T); + } + else + { +#ifdef DEBUG_QUAT_MATH + printf("*** 0 degree turn!!!\n"); +#endif + // Just reuse the previous orient, which we preloaded into M. + return r; + } + } + + // Convert to a quaternion, and convert that to a rotation matrix. + axisangle2quat(t, r, q); + normalizequat( q ); + quat2matrix(q, m); + + matrixmult(m, M); // Combine rotation m with previous rotation M. + + // Move temp vars into return values. + matrixcp(M,m); + vectorcp(T, t); + return r; +} + +//********************************************************************** +// Convert start, end constraints and n points to oriented segments +// using quaternion. +//********************************************************************** +void +orientq( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION start_up[3],end_up[3],up[3]; + PRECISION start_v[3],end_v[3],v[3]; + PRECISION start_q[4],end_q[4],q[4]; + PRECISION m[3][3]; + PRECISION total_length = hose_length(n_segments,segments); + PRECISION cur_length; + PRECISION r, a; + PRECISION front[3]; + PRECISION t[3]; + int i; + +//#ifdef DEBUG_QUAT_MATH + PRECISION pi = 2*atan2(1,0); + PRECISION degrees = 180.0 / pi; +//#endif + +#if 0 + // Use simple linear up vector interpolation for 2 segs or less. + // Eventually I may need to do something to break these up if spin > 1 degree. + if (n_segments <= 2) + { + orient(start, end, n_segments, segments); + return; + } +#endif + + // Get the start and end velocity vectors. + // The basic HoseSeg.dat files extend the hose along the +Y axis. + start_v[0] = end_v[0] = 0; + start_v[1] = end_v[1] = 1; // Use positive Y like hose part. + start_v[1] = end_v[1] = -1; // Use negative Y like constraint part. + start_v[2] = end_v[2] = 0; + // The hose parts actually grow in the +Y direction (ldraw down). + // The chain link is the oddball here, but we can slide it down 10 to match. + // So it's normal for the parts to point backwards? Test this again. + // + // I see both ends of some hose parts are now capped, but not the ribbed hose. + // So it really is necessary to flip the end part, sometimes. + // But do it in lsynth.mpd. + // + // The problem with the end of the brown hose is that you only get one part + // at the one end (after segment reduction) + // It's the start AND the end part, and the LAST end part. What do you do with it? + + //****** Get velocity vectors from the Start and End constraints. + rotate_point(start_v,start->orient); + rotate_point(end_v,end->orient); + normalize(start_v); // Normalize again, + normalize(end_v); // Just in case orient includes a stretch. + + //****** Get the up vectors from the Start and End constraints. + // The fin on LS00.dat is along -Z so rotate that. + start_up[0] = end_up[0] = 0; + start_up[1] = end_up[1] = 0; + start_up[2] = end_up[2] = -1; + rotate_point(start_up,start->orient); + rotate_point(end_up,end->orient); + normalize(start_up); // Normalize again, + normalize(end_up); // Just in case orient includes a stretch. + +#ifdef DEBUG_QUAT_MATH + printf("V[S] = (%.2fx, %.2fy, %.2fz)\n", start_v[0],start_v[1],start_v[2]); + printf("M[S] = (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f)\n", + start->orient[0][0], + start->orient[0][1], + start->orient[0][2], + start->orient[1][0], + start->orient[1][1], + start->orient[1][2], + start->orient[2][0], + start->orient[2][1], + start->orient[2][2]); +#endif + + /* Calculate turn axis (t) and angle (r) for each segment. + * Use this to calculate an orient matrix for each segment. + * Preload last good vals for special cases like 0 and 180 degree turns. + ********************************************/ + for (i = 0; i < n_segments-1; i++) { + + // Get the Velocity vector for Segment[i]. + v[0] = segments[i+1].offset[0] - segments[i].offset[0]; + v[1] = segments[i+1].offset[1] - segments[i].offset[1]; + v[2] = segments[i+1].offset[2] - segments[i].offset[2]; + normalize(v); + + // Preload segments[i].orient with previous orient in case of 0 degree turn. + // Preload turn axis t with previous t (or up) in case of 180 degree turn. + if (i) + { + matrixcp(segments[i].orient, segments[i-1].orient); + // Reuse the previous t. + } + else + { + matrixcp(segments[i].orient, start->orient); + vectorcp(t, start_up); // Default t to the up vector. + } + + // Get a turn matrix from Start velocity vector to the velocity of Segment[i]. + // Pass in some preloaded matrices and such, for special cases like 0 or 180. + // NOTE: special cases are less likely if we calculate incremental turns. + // (Get turn from seg[i-1] to seg[i] instead of from start to seg[i].) + r = get_turn_mat(segments[i].orient, start_v, v, t); + +#ifdef DEBUG_QUAT_MATH + if (i == 0) + { + printf(" U[S] = (%.2fx, %.2fy, %.2fz)", start_up[0],start_up[1],start_up[2]); + printf(" V[S] = (%.2fx, %.2fy, %.2fz)\n", start_v[0],start_v[1],start_v[2]); + } + up[0] = 0; + up[1] = 0; + up[2] = -1; + rotate_point(up,segments[i].orient); + normalize(up); // Normalize again + printf(" u[%d] = (%.2fx, %.2fy, %.2fz)", i, up[0],up[1],up[2]); + printf(" V[%d] = (%.2fx, %.2fy, %.2fz)\n", i, v[0],v[1],v[2]); +#endif + + // Use incremental turns. Pass v[i-1] instead of using start_v. + vectorcp(start_v, v); + +#ifdef DEBUG_QUAT_MATH + // Interpolate Up based on length from Start to Segment[i]. (For debug only.) + cur_length = hose_length(i,segments); + cur_length /= total_length; + up[0] = start_up[0]*(1-cur_length) + end_up[0]*cur_length; + up[1] = start_up[1]*(1-cur_length) + end_up[1]*cur_length; + up[2] = start_up[2]*(1-cur_length) + end_up[2]*cur_length; + normalize(up); + + printf("\t %.2f degree TURN about (%.2f, %.2f, %.2f)\n", + r*degrees, t[0],t[1],t[2]); +#if 0 + printf("M[%d] = (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f)\n", + i, + segments[i].orient[0][0], + segments[i].orient[0][1], + segments[i].orient[0][2], + segments[i].orient[1][0], + segments[i].orient[1][1], + segments[i].orient[1][2], + segments[i].orient[2][0], + segments[i].orient[2][1], + segments[i].orient[2][2]); +#endif +#endif + + } // Segments[i].orient should now contain rotation matrices. + +#ifdef DEBUG_QUAT_MATH + printf("V[E] = (%.2fx, %.2fy, %.2fz)\n", end_v[0],end_v[1],end_v[2]); + printf("M[E] = (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f)\n", + end->orient[0][0], + end->orient[0][1], + end->orient[0][2], + end->orient[1][0], + end->orient[1][1], + end->orient[1][2], + end->orient[2][0], + end->orient[2][1], + end->orient[2][2]); +#endif + + // Now we should be able to add the spin + // Rotate start_up by segments[n].orient + // Use dot product to get the extra spin angle between that and end_up. + // Spread out the spin (as a pre-rotation around Y) over the length of hose. + // Actually, we can calculate this extra spin ahead of time + // and do the pre-rotation inside the for loop we already have. + +#ifdef DEBUG_QUAT_MATH + printf("U[S] = (%.2fx, %.2fy, %.2fz)\n", start_up[0],start_up[1],start_up[2]); + i = n_segments-2; + printf("M[%d] = (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f)\n", + i, + segments[i].orient[0][0], + segments[i].orient[0][1], + segments[i].orient[0][2], + segments[i].orient[1][0], + segments[i].orient[1][1], + segments[i].orient[1][2], + segments[i].orient[2][0], + segments[i].orient[2][1], + segments[i].orient[2][2]); +#endif + + //****** Get the Final rotation matrix m. (Should be segments[n-1].orient). + matrixcp(m, segments[n_segments-2].orient); + // Reuse the previous t (and previous start_v). + r = get_turn_mat(m, start_v, end_v, t); + + //****** Rotate up vector by Final rotation matrix to get basis for end twist. + // I know. I can't use segments[n-2].orient because that includes + // the rotation in start->orient. If I want to use that I have to + // pass it the unmodified -Z up vector. Not the rotated start_up. + up[0] = 0; + up[1] = 0; + up[2] = -1; + rotate_point(up,m); + normalize(up); // Normalize again + +#if 0 + printf("ROTATE U[-z] %.2fdeg about (%.2f, %.2f, %.2f)\n", + r*degrees, t[0],t[1],t[2]); +#endif + + // The rotated u[S] should match the U[E], unless there is some extra twist. +#ifdef DEBUG_QUAT_MATH + printf(" U[S] = (%.2fx, %.2fy, %.2fz)\n", start_up[0],start_up[1],start_up[2]); + printf(" U[P] = (%.2fx, %.2fy, %.2fz)\n", up[0],up[1],up[2]); + printf(" U[E] = (%.2fx, %.2fy, %.2fz)\n", end_up[0],end_up[1],end_up[2]); +#endif + + //Dot product gives turn angle. a.b=|a||b|cos(theta) + //We normalized so |a|=|b|=1, which means theta = acos(a.b). + r = dotprod(up, end_up); + // Warning!! acos() will give NAN if we give it badly normalized numbers. + if (r > 1.0) + r = 1.0; + if (r < -1.0) + r = -1.0; + r = acos(r); + + //Cross product gives turn axis. + mult_point(t, up, end_up); + normalize(t); + + // Check sign of turn to see if it's left or right. + a = dotprod(t, end_v); + if (a > 0.0) + r = -r; // Rotate the opposite way. + +#ifdef DEBUG_QUAT_MATH + printf("%.2f degree TWIST about (%.2f, %.2f, %.2f)\n\n", + r*degrees, t[0],t[1],t[2]); + + //***************************************************************** + // Also, when I get everything else working, consider this... + // I'm still getting a small TURN around the side axis (-X) + // that must be eliminated before we can isolate the extra TWIST. + // Probably the difference between the last segment we calculated + // using the bezier curve() fn and the orient of the end constraint. + // Can we force the bezier fn to make the start and end segments + // match the orient of the start and end constraints. The original + // code just replaces the orient of the first and last segments. + // That's OK for very short segments, but could give a rather sharp + // Turn for the 2nd and 2nd to last segments if they have any + // length to them. +#endif + + //***************************************************************** + // Ok now we have the extra up vector twist angle in r. + // Interpolate it over the length of the hose. + // I'm not sure this is quite right, but it goes from 0 to 100%. + // Probably ought to draw a pretty picture to be absolutely sure. + //***************************************************************** + total_length = hose_length(n_segments-1,segments); + for (i = 1; i < n_segments; i++) { + + // Interpolate the twist (if there is any) over the length of segment. + if ((r != 0.0) && (total_length != 0)) + { + cur_length = hose_length(i,segments); + cur_length /= total_length; + a = r * cur_length; + +#ifdef DEBUG_QUAT_MATH + printf("TWIST[%d] at length %.2f of %.2f = %.2fdeg \n", + i, cur_length, total_length, a*degrees); +#endif + + m[0][0] = 1; + m[0][1] = 0; + m[0][2] = 0; + m[1][0] = 0; + m[1][1] = 1; + m[1][2] = 0; + m[2][0] = 0; + m[2][1] = 0; + m[2][2] = 1; + + m[0][0] = cos(a); + m[0][2] = sin(a); + m[2][0] = -sin(a); + m[2][2] = cos(a); + + matrixmult(segments[i-1].orient, m); + } + + //***************************************************************** + // Last but not least, make the hoses parts grow backwards. + //***************************************************************** + // The constraints point towards -Y (up in ldraw) + // But the hose segs point towards +Y (down in ldraw) + // I cannot mix and match the constraint orients with hose orients. + // I absolutely must figure out whether I want hose segs to point + // forwards or backwards, and then use that to do this. + // It'd be a whole lot easier for me if they all pointed forwards... + // To do that I'd make them point (or grow) toward -Y via lsynth.mpd. + // How does that affect the finished ends of the hose? It shouldn't. + //***************************************************************** + m[0][0] = 1; + m[0][1] = 0; + m[0][2] = 0; + m[1][0] = 0; + m[1][1] = -1; // Flip the Y coord. + m[1][2] = 0; + m[2][0] = 0; + m[2][1] = 0; + m[2][2] = 1; + matrixmult(segments[i-1].orient, m); + } + + // We still have to flip the Y, even if only one segment. + if (n_segments == 1) + { + m[0][0] = 1; + m[0][1] = 0; + m[0][2] = 0; + m[1][0] = 0; + m[1][1] = -1; // Flip the Y coord. + m[1][2] = 0; + m[2][0] = 0; + m[2][1] = 0; + m[2][2] = 1; + matrixmult(segments[0].orient, m); + } + +} + diff --git a/lsynth/src/curve.h b/lsynth/src/curve.h new file mode 100644 index 000000000..ab8b9beaa --- /dev/null +++ b/lsynth/src/curve.h @@ -0,0 +1,44 @@ +/* + * This file describes the interface to the LDRAW synthesizable parts library. + * Kevin Clague + */ +#ifndef CURVE_H +#define CURVE_H + +#ifdef _cplusplus +extern "C" { +#endif + +int +synth_curve( + part_t *start, + part_t *end, + part_t *segments, + int n_segments, + PRECISION attrib, + FILE *output); + +PRECISION +hose_length( + int n_segments, + part_t *segments); + +void +orient( + part_t *start, + part_t *end, + int n_segments, + part_t *segments); + +void +orientq( + part_t *start, + part_t *end, + int n_segments, + part_t *segments); + +#ifdef _cplusplus +}; +#endif + +#endif diff --git a/lsynth/src/curvy.c b/lsynth/src/curvy.c new file mode 100644 index 000000000..2a12d53cf --- /dev/null +++ b/lsynth/src/curvy.c @@ -0,0 +1,440 @@ +/* + * This is the LDRAW parts synthesis library. + * By Kevin Clague + */ + +#include "lsynthcp.h" +#include "hose.h" +#include "curve.h" +#include "mathlib.h" + +/* + * a 1x1 brick is 20 LDU wide and 24 LDU high + * + * hose length = 14 brick widths long = 280 LDU + * number of ribs = 45 + * 6.2 LDU per rib + * + */ + +void orient( + int n_segments, + part_t *segments) +{ + int i; + + for (i = 0; i < n_segments-1; i++) { + + PRECISION dx, dy, dz; + PRECISION orient[3][3]; + PRECISION L1,L2; + + // determine the orientation of the part in the XY plane + // WARNING! This is bad for twist if the hose mostly bends in XZ or YZ plane. + // Start with the orient of the 1st constraint (the fin), and adjust from there? + // Quaternion roll interpolation? + // Convert constraint orientations to quaternions and slerp the points between? + + dx = segments[i+1].offset[0] - segments[i].offset[0]; + dy = segments[i+1].offset[1] - segments[i].offset[1]; + dz = segments[i+1].offset[2] - segments[i].offset[2]; + + L1 = sqrt(dx*dx + dy*dy); + L2 = sqrt(dx*dx + dy*dy + dz*dz); + if ((L1 == 0) || (L2 == 0)){ // Avoid divide by 0. + segments[i].orient[0][0] = 1; + segments[i].orient[1][0] = 0; + segments[i].orient[2][0] = 0; + segments[i].orient[0][1] = 0; + segments[i].orient[1][1] = 0; + segments[i].orient[2][1] =-1; + segments[i].orient[0][2] = 0; + segments[i].orient[1][2] = 1; + segments[i].orient[2][2] = 0; + } else { + segments[i].orient[0][0] = dy/L1; // cos + segments[i].orient[1][0] = -dx/L1; // sin + segments[i].orient[2][0] = 0; + segments[i].orient[0][1] = dx/L2; // -sin + segments[i].orient[1][1] = dy/L2; // cos + segments[i].orient[2][1] = dz/L2; + segments[i].orient[0][2] = -dx*dz/(L1*L2); + segments[i].orient[1][2] = -dy*dz/(L1*L2); + segments[i].orient[2][2] = L1/L2; + } +#if 1 + if ((i== 0) && (n_segments < 100)) + printf("Orient seg[%d](%.1f, %.1f, %.1f) = \n (%.1f, %.1f, %.1f) (%.1f, %.1f, %.1f) (%.1f, %.1f, %.1f)\n", + i, dx,dy,dz, + segments[i].orient[0][0], + segments[i].orient[1][0], + segments[i].orient[2][0], + segments[i].orient[0][1], + segments[i].orient[1][1], + segments[i].orient[2][1], + segments[i].orient[0][2], + segments[i].orient[1][2], + segments[i].orient[2][2]); +#endif + + // Try the more robust? (but ugly) reorient code from ldglite hoser fn. + // This does not seem to do any better. (also operates in XY plane?) + if (n_segments < 100) + { + double Xaxisrotation,Yaxisrotation,pi,pidiv2; + pi=acos(-1); + pidiv2=pi/2; + + if (dx<0){ + if (dy<0){ + Xaxisrotation=-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + if (dz<0) { + Yaxisrotation=(atan((dx)/(dz)));} + else if (dz>0) { + Yaxisrotation=(-pi+(atan((dx)/(dz))));} + else { + Yaxisrotation=pidiv2;} + } + else if (dy>0){ + if (dz<0) { + Xaxisrotation=pi-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=(atan((dx)/(dz)));} + else if (dz>0) { + Xaxisrotation=pi-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=(-pi+(atan((dx)/(dz))));} + else { + Xaxisrotation=(-pi+(atan((dx)/(dy)))); + Yaxisrotation=pidiv2;} + } + else {// if (dy==0) + Xaxisrotation=pidiv2; + if (dz<0) { + Yaxisrotation=(pi+(atan((dx)/(dz))));} + else if (dz>0) { + Yaxisrotation=(pi+(atan((dx)/(dz))));} + else { + Yaxisrotation=pidiv2;} + } + } + //*********************************************************** + else if (dx>0){ + if (dy<0){ + Xaxisrotation=-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + if (dz<0) { + Yaxisrotation=(atan((dx)/(dz)));} + else if (dz>0) { + Yaxisrotation=(-pi+(atan((dx)/(dz))));} + else { + Yaxisrotation=pidiv2;} + } + else if (dy>0){ + Xaxisrotation=pi-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + if (dz<0) { + Yaxisrotation=(atan((dx)/(dz)));} + else if (dz>0) { + Yaxisrotation=(-pi+(atan((dx)/(dz))));} + else { + Yaxisrotation=pidiv2;} + } + else { // if (dy==0) + Xaxisrotation=-pidiv2; + if (dz<0) { + Yaxisrotation=(atan((dx)/(dz)));} + else if (dz>0) { + Yaxisrotation=(atan((dx)/(dz)));} + else { + Yaxisrotation=-pidiv2;} + } + } + //*********************************************************** + else{ // if (dx==0) + if (dy<0){ + if (dz<0) { + Xaxisrotation=-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=0;} + else if (dz>0) { + Xaxisrotation=(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=0;} + else { + Xaxisrotation=0; + Yaxisrotation=pi;} + } + else if (dy>0){ + if (dz<0) { + Xaxisrotation=pi-(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=0;} + else if (dz>0) { + Xaxisrotation=-pi+(atan(sqrt(((dx)*(dx))+((dz)*(dz)))/(dy))); + Yaxisrotation=0;} + else { + Xaxisrotation=pi; + Yaxisrotation=pi;} + } + else { // if (dy==0) + if (dz<0) { + Xaxisrotation=pidiv2; + Yaxisrotation=0;} + else if (dz>0) { + Xaxisrotation=-pidiv2; + Yaxisrotation=0;} + else { + Xaxisrotation=pi; + Yaxisrotation=pi;} + } + } + + // Turn the parts 180 degrees to make them face the same way. + if (Xaxisrotation < 0) + Xaxisrotation += pi; + else + Xaxisrotation -= pi; + + segments[i].orient[0][0] = cos(Yaxisrotation); + segments[i].orient[0][1] = (sin(Xaxisrotation)*sin(Yaxisrotation)); + segments[i].orient[0][2] = (cos(Xaxisrotation)*sin(Yaxisrotation)); + segments[i].orient[1][0] = 0; + segments[i].orient[1][1] = cos(Xaxisrotation); + segments[i].orient[1][2] = -sin(Xaxisrotation); + segments[i].orient[2][0] = -sin(Yaxisrotation); + segments[i].orient[2][1] = (sin(Xaxisrotation)*cos(Yaxisrotation)); + segments[i].orient[2][2] = (cos(Xaxisrotation)*cos(Yaxisrotation)); + + if ((i== 0) && (n_segments < 100)) + printf("ORIENT seg[%d](%.1f, %.1f, %.1f) = \n (%.1f, %.1f, %.1f) (%.1f, %.1f, %.1f) (%.1f, %.1f, %.1f)\n", + i, dx,dy,dz, + segments[i].orient[0][0], + segments[i].orient[1][0], + segments[i].orient[2][0], + segments[i].orient[0][1], + segments[i].orient[1][1], + segments[i].orient[2][1], + segments[i].orient[0][2], + segments[i].orient[1][2], + segments[i].orient[2][2]); + } + } +} + + +#if 0 + // quaternion(a,b,c,s) is just a direction vector with a spin(roll) angle. + // ie. quaternion(x,y,z,spin) + + // convert rotation matrix m[16] to quaternion(a,b,c,s). + // Calculate the trace of the matrix T from the equation: + T = 1 + m[0] + m[5] + m[10]; + if ( T > 0.00000001 ) + { + S = sqrt(T) * 2; + a = ( m[9] - m[6] ) / S; + b = ( m[2] - m[8] ) / S; + c = ( m[4] - m[1] ) / S; + s = 0.25 * S; + } + // If the trace of the matrix is equal to zero then identify + // which major diagonal element has the greatest value. + // Depending on this, calculate the following: + else if ( m[0] > m[5] && m[0] > m[10] ) { // Column 0: + S = sqrt( 1.0 + m[0] - m[5] - m[10] ) * 2; + a = 0.25 * S; + b = (m[4] + m[1] ) / S; + c = (m[2] + m[8] ) / S; + s = (m[9] - m[6] ) / S; + } else if ( m[5] > m[10] ) { // Column 1: + S = sqrt( 1.0 + m[5] - m[0] - m[10] ) * 2; + a = (m[4] + m[1] ) / S; + b = 0.25 * S; + c = (m[9] + m[6] ) / S; + s = (m[2] - m[8] ) / S; + } else { // Column 2: + S = sqrt( 1.0 + m[10] - m[0] - m[5] ) * 2; + a = (m[2] + m[8] ) / S; + b = (m[9] + m[6] ) / S; + c = 0.25 * S; + s = (m[4] - m[1] ) / S; + } + + // convert quaternion(a,b,c,s) to a rotation matrix m[16]. + m[0] = 1 - 2*b*b-2*c*c; + m[1] = 2*a*b - 2*s*c; + m[2] = 2*a*c + 2*s*b; + m[4] = 2*a*b+2*s*c; + m[5] = 1 - 2*a*a - 2*c*c; + m[6] = 2*b*c - 2*s*a; + m[8] = 2*a*c - 2*s*b; + m[9] = 2*b*c + 2*s*a; + m[10] = 1 - 2*a*a - 2*b*b; + m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0; + m[15] = 1; +#endif + + + +int +synth_curve( + part_t *start, + part_t *end, + part_t *segments, + int n_segments, + PRECISION attrib, + FILE *output) +{ + PRECISION vector[3]; + PRECISION start_speed_v[3]; + PRECISION stop_speed_v[3]; + PRECISION time,time2,i_time,i_time_sum,i_time_sum2,n_time; + PRECISION x,x2,y,y2,z,z2; + PRECISION ptp,ratio; + PRECISION ptp_sum; + int i,j,n; + PRECISION up[3]; + + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x += start->orient[j][i] * vector[i]; + } + start_speed_v[j] = x; + } + + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x -= end->orient[j][i] * vector[i]; + } + stop_speed_v[j] = x; + } + + // stop_speed_v[2] = -stop_speed_v[2]; + + for (i = 0; i < n_segments; i++) { + time = (PRECISION) i/ (PRECISION) n_segments; + + segments[i].offset[0] = + (1 - time) * (1 - time) * (1 - time) * start->offset[0] + + (1 - time) * (1 - time) * 3 * time * (start->offset[0] - start_speed_v[0]) + + (1 - time) * 3 * time * time * (end->offset[0] - stop_speed_v[0]) + + time * time * time * end->offset[0]; + + segments[i].offset[1] = + (1 - time) * (1 - time) * (1 - time) * start->offset[1] + + (1 - time) * (1 - time) * 3 * time * (start->offset[1] - start_speed_v[1]) + + (1 - time) * 3 * time * time * (end->offset[1] - stop_speed_v[1]) + + time * time * time * end->offset[1]; +/* +=(1-$A8)^3*D$4 + (1-$A8)^2*3*$A8*(D$4-D$3) + (1-$A8)*3*$A8^2*(D$5-D$6) + $A8^3*D$5 + */ + segments[i].offset[2] = + (1 - time) * (1 - time) * (1 - time) * start->offset[2] + + (1 - time) * (1 - time) * 3 * time * (start->offset[2] - start_speed_v[2]) + + (1 - time) * 3 * time * time * (end->offset[2] - stop_speed_v[2]) + + time * time * time * end->offset[2]; + } + + ptp_sum = 0; + + for (i = 0; i < n_segments - 1; i++) { + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + ptp_sum += ptp; + } + + /* G8 */ + + i_time_sum = 0; + for (i = 0; i < n_segments - 1; i++) { + time = (PRECISION) (i+1)/ (PRECISION) n_segments; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + + ratio = ptp*n_segments/ptp_sum; + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); + i_time_sum += i_time; + } + + /* H, I, J */ + n_time = 0; + i_time_sum2 = 0; + for (i = 0; i < n_segments - 1; i++) { + PRECISION foo; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x * x + y * y + z * z); /* E */ + ratio = ptp*n_segments/ptp_sum; /* F */ + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); /* G */ + i_time_sum2 += i_time; + + foo = 1.0/n_segments; + foo /= ratio; + foo /= i_time_sum; + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + n_time += foo; /* H */ + } + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + orient(n_segments, segments); + + return 0; /* it all worked */ +} + diff --git a/lsynth/src/hose.c b/lsynth/src/hose.c new file mode 100644 index 000000000..c6b722fd4 --- /dev/null +++ b/lsynth/src/hose.c @@ -0,0 +1,889 @@ +/* + * This is the LDRAW parts synthesis library. + * By Kevin Clague and Don Heyse + */ + +#include + +#include "lsynthcp.h" +#include "hose.h" +#include "curve.h" +#include "mathlib.h" + +#define PI 2*atan2(1,0) + +/* + * 0 SYNTH BEGIN DEFINE HOSE PNEUMATIC_HOSE "descr" + * 1 a b c d e f g h i j k l + * 1 a b c d e f g h i j k l + * 0 SYNTH END + */ + +int n_hose_types = 0; +hose_attrib_t hose_types[64]; + +#define N_HOSE_TYPES n_hose_types + +/* In hoses, the attrib field in constraints, indicates that + * LSynth should turn the final constraint around to get everything + * to work correctly (e.g. flex-axle ends). + */ + +/* + * 0 SYNTH BEGIN DEFINE HOSE CONSTRAINTS + * 1 a b c d e f g h i j k l + * 1 a b c d e f g h i j k l + * 0 SYNTH END + */ + +int n_hose_constraints = 0; +part_t hose_constraints[128]; + +#define N_HOSE_CONSTRAINTS n_hose_constraints + +void +list_hose_types(void) +{ + int i; + + printf("\n\nHose like synthesizable parts\n"); + for (i = 0; i < N_HOSE_TYPES; i++) { + printf(" %-20s %s\n",hose_types[i].type, hose_types[i].descr); + } +} + +void +list_hose_constraints(void) +{ + int i; + + printf("\n\nHose constraints\n"); + for (i = 0; i < N_HOSE_CONSTRAINTS; i++) { + printf(" %11s\n",hose_constraints[i].type); + } +} + +void +hose_ini(void) +{ + int i; + + for (i = 0; i < N_HOSE_TYPES; i++) { + printf("%-20s = SYNTH BEGIN %s 16\n",hose_types[i].type, hose_types[i].type); + } +} + +int +ishosetype(char *type) +{ + int i; + + for (i = 0; i < N_HOSE_TYPES; i++) { + if (strncasecmp(hose_types[i].type,type,strlen(hose_types[i].type)) == 0) { + return 1; + } + } + return 0; +} +// casecmp +int +ishoseconstraint(char *type) +{ + int i; + + for (i = 0; i < N_HOSE_CONSTRAINTS;i++) { + if (strcasecmp(hose_constraints[i].type,type) == 0) { + return 1; + } + } + return 0; +} + +PRECISION +line_angle( + PRECISION va[3], + PRECISION vb[3]) +{ + PRECISION denom; + PRECISION theta; + + denom = vectorlen(va)*vectorlen(vb); + + if (fabs(denom) < 1e-9) { + //printf("line angle calculation gives us length of zero\n"); + return 0; + } + + theta = (va[0]*vb[0]+va[1]*vb[1]+va[2]*vb[2])/denom; + if (theta >= 1 || theta <= -1) { + theta = 0; + } else { + theta = acos(theta); + } + return theta; +} + +PRECISION +line_angle3( + int a, + int b, + part_t *segments) +{ + PRECISION va[3]; /* line A */ + PRECISION vb[3]; /* line B */ + PRECISION denom; + PRECISION theta; + + /* we get the line A as point[a] to point[a+1] */ + /* and line B as point[b] to point[b+1] */ + + vectorsub3(va,segments[a+1].offset,segments[a].offset); + vectorsub3(vb,segments[b+1].offset,segments[b].offset); + + return line_angle(va,vb); +} + +/* + * merge adjacent points until we either experience too much + * bend, or too much twist + */ + +int +merge_segments_angular( + part_t *start, + part_t *end, + part_t *segments, + int *n_segments, + PRECISION max_bend, + PRECISION max_twist, + FILE *output) +{ + int a,b; + int n; + PRECISION theta1,theta2; + PRECISION total_length, cur_length; + PRECISION start_up[3] = { 1, 0, 0 }; + PRECISION end_up[3] = { 1, 0, 0 }; + PRECISION cur_up[3] = { 1, 0, 0 }; + PRECISION next_up[3]; + PRECISION len[3], sub_len; + + vectorrot(start_up,start->orient); + vectorrot(cur_up, start->orient); + vectorrot(end_up, end->orient); + total_length = hose_length(*n_segments, segments); + cur_length = 0; + + a = 0; b = 1; + n = 0; + + do { + vectorsub3(len,segments[a].offset,segments[b].offset); + sub_len = vectorlen(len); + if (sub_len < 1e-9) { + b++; + } + } while (sub_len < 1e-9); + + while (b < *n_segments) { + PRECISION len[3],normalized; + + if (b < *n_segments - 1) { + vectorsub3(len,segments[b+1].offset,segments[b].offset); + cur_length += vectorlen(len); + } else { + cur_length += (total_length - cur_length)/2; + } + + if (cur_length != 0) { + PRECISION left; + + normalized = cur_length/total_length; + left = 1 - normalized; + + next_up[0] = start_up[0]*left + end_up[0]*normalized; + next_up[1] = start_up[1]*left + end_up[1]*normalized; + next_up[2] = start_up[2]*left + end_up[2]*normalized; + + normalized = next_up[0]*next_up[0] + + next_up[1]*next_up[1] + + next_up[2]*next_up[2]; + normalized = sqrt(normalized); + next_up[0] /= normalized; + next_up[1] /= normalized; + next_up[2] /= normalized; + + theta2 = line_angle(cur_up,next_up); + theta1 = line_angle3(a,b,segments); + + if (theta1 < max_bend && theta2 < max_twist) { + b++; + } else { + segments[n++] = segments[a++]; + a = b++; + cur_up[0] = next_up[0]; + cur_up[1] = next_up[1]; + cur_up[2] = next_up[2]; + } + } else { + b++; + } + } + if (n <= 2) { + segments[1] = segments[*n_segments-1]; + n = 2; + } else if (b - a > 1) { + //n--; // -= 2; + segments[n-1] = segments[a]; + } else { + n--; + } + *n_segments = n; + return 0; +} + +int +merge_segments_length( + part_t *segments, + int *n_segments, + PRECISION max, + FILE *output) +{ + int a,b; + int n; + PRECISION d[3],l; + + a = 0; b = 1; + n = 1; + + while (b < *n_segments) { + + vectorsub3(d,segments[a].offset,segments[b].offset); + l = vectorlen(d); + + if (l + 0.5 < max) { + b++; + } else { + a = b; + segments[n++] = segments[b++]; + } + } + if (n < 2) { + segments[1] = segments[*n_segments-1]; + n = 2; + } else if (b - a > 1) { + //n--; // -= 2; + segments[n-1] = segments[a]; + } else { + n--; + } + *n_segments = n; + return 0; +} + +int +merge_segments_count( + hose_attrib_t *hose, + part_t *start, + part_t *end, + part_t *segments, + int *n_segments, + int count, + FILE *output) +{ + int n, i; + PRECISION d[3],l; + PRECISION len, lenS, lenM, lenE; + + // Get the total length of the curve and divide by the expected segment count. + len = 0; + for (i = 0; i < *n_segments-1; i++) { + vectorsub3(d,segments[i].offset,segments[i+1].offset); + len += vectorlen(d); + } + printf("Total segment len = %.3f\n", len); + + // If S or E do not match the N parts subtract their lengths from total. + if ((strcasecmp(hose->start.type, hose->mid.type) != 0) || + (strcasecmp(hose->end.type, hose->mid.type) != 0) || + (hose->start.attrib != hose->mid.attrib) || + (hose->end.attrib != hose->mid.attrib)) + { + lenS = hose->start.attrib; + lenE = hose->end.attrib; + lenM = hose->mid.attrib; + len = len - (lenS + lenE); + printf("Net segment len = %.3f (S=%d, M=%d, E= %d)\n", len, lenS, lenM, lenE); + len = len / (PRECISION)(count-1); //len /= (count); + } + else + { + len = len / (PRECISION)(count-1); //len /= (count); + lenS = lenE = len; + } + printf("Merging %d segments to %d segments of len %.3f\n", *n_segments, count, len); + + // Break up the curve into count intervals of length len. + l = 0; + n = 1; // Keep the first point. + // Find intermediate points. + for (i = 0; i < *n_segments-1; i++) { + vectorsub3(d,segments[i].offset,segments[i+1].offset); + l += vectorlen(d); + + if ((l + 0.05) > (((n-1) * len) + lenS)) + segments[n++] = segments[i+1]; + + //if (n >= count) break; + } + + if (lenE != len) // If E did not match above, place it at the constraint. + { + segments[n-1] = segments[*n_segments-1]; // Use the last point twice? + } + segments[n++] = segments[*n_segments-1]; // Keep the last point. + *n_segments = n; + + // NOTE: What I really need to do here is place the last point + // segments[n-1].offset at the location of the end constraint + a lenE + // offset in the direction of the orientation of the end constraint. + // otherwise the final vector is tiny and rounding can point it backwards. + // We can see a backwards vector by checking for a negative dot product. + + // So, move the last point lenE from the start of the End constraint. + // This should place it at the far point of the end constraint. + d[0] = 0; d[1] = lenE; d[2] = 0; // Create an offset vector of lenE + d[1] *= -1; // along the -Y axis. Reorient it + vectorrot(d,end->orient); // along the end constraint axis, and + vectoradd(segments[n-1].offset,d); // add it to the end constraint origin. + + if (0) // Debug printouts + { + extern PRECISION dotprod(PRECISION a[3], PRECISION b[3]); // from curve.c + + PRECISION last, next; + PRECISION dn[3],dp; + PRECISION m2[3][3]; + + matrixcp(m2,end->orient); + printf(" E =(%g, %g, %g, %g, %g, %g, %g, %g, %g)\n", m2[0][0], m2[0][1], m2[0][2], + m2[1][0], m2[1][1], m2[1][2], m2[2][0], m2[2][1], m2[2][2]); + printf(" d=(%g, %g, %g)", d[0], d[1], d[2]); + + vectorsub3(d,segments[n-1].offset,segments[n-2].offset); + last = vectorlen(d); + vectorsub3(dn,segments[n-2].offset,segments[n-3].offset); + next = vectorlen(dn); + dp = dotprod(dn,d); + printf(" last=%g, next=%g, dp=%g\n", last, next, dp); + } + + printf("Produced %d points (%d segments)\n", *n_segments, *n_segments-1); + + // Reorient the segments. + // Warning! Can interact badly with twist if hose makes a dx/dz (dy=0) turn. + // Also, I think this ignores the orientation of the start and end constraints. + // The fin on the constraints should guide the orientation (or twist?) somehow. + // orient(n,segments); + return 0; +} + +#define MAX_SEGMENTS 1024*8 + +part_t segments[MAX_SEGMENTS]; + +// Create a second list to combine all patches between constraints. +part_t seglist[MAX_SEGMENTS]; + +void +output_line( + FILE *output, + int ghost, + char *group, + int color, + PRECISION a, + PRECISION b, + PRECISION c, + PRECISION d, + PRECISION e, + PRECISION f, + PRECISION g, + PRECISION h, + PRECISION i, + PRECISION j, + PRECISION k, + PRECISION l, + char *type) +{ + if (group) { + fprintf(output,"0 MLCAD BTG %s\n",group); + } + fprintf(output,"%s1 %d %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %1.4f %s\n", + ghost ? "0 GHOST " : "", + color, + a,b,c,d,e,f,g,h,i,j,k,l, + type); + group_size++; + fflush(stdout); +} + +/* + * Twist + * cos(t) 0 sin(t) + * 0 1 0 + * -sin(t) 0 cos(t) + * + * We need to add orientation of the segments based on the orientation + * of the constraint. We must bring the constraint into the normalized + * orientation of the constraint (pointing up at Y), and determine the + * angle of the tab relative to the Y axis. + */ + +void +render_hose_segment( + hose_attrib_t *hose, + int ghost, + char *group, + int *group_size, + int color, + part_t *segments, + int n_segments, + PRECISION *total_twist, + int first, + int last, + FILE *output, + part_t *constraint) +{ + int i,j,k; + PRECISION pi = 2*atan2(1,0); + PRECISION m1[3][3]; + PRECISION m2[3][3]; + PRECISION offset[3]; + char *type; + int gs = *group_size; + + for (i = 0; i < n_segments-1; i++) { + PRECISION tx,ty,tz,l,theta; + + if (hose->fill != STRETCH) { + l = 1; + } else { + PRECISION d[3]; + + vectorsub3(d,segments[i+1].offset,segments[i].offset); + + // the length of the segment is the distance between the + // two points + + l = vectorlen(d); + + // plus the length needed to make sure that the outer edges + // of the cross sections touch + + if (n_segments > 1) { + PRECISION phi; + + phi = line_angle3(i+1,i,segments); + phi = sin(phi)*(hose->diameter); + + l += phi + phi; + } + } + + m1[0][0] = 1; + m1[0][1] = 0; + m1[0][2] = 0; + m1[1][0] = 0; + m1[1][1] = l; // Stretch it by length of segment. + m1[1][2] = 0; + m1[2][0] = 0; + m1[2][1] = 0; + m1[2][2] = 1; + + // Default offset is nothing. + offset[0] = 0; offset[1] = 0; offset[2] = 0; + + if (i == 0 && first) { + type = hose->start.type; + vectorcp(offset,hose->start.offset); + if (hose->start.attrib != 0) // FIXED size start, do not stretch it. + matrixcp(m2,hose->start.orient); + else // Stretch it. + matrixmult3(m2,hose->start.orient,m1); + } else if (i == n_segments-2 && last) { + type = hose->end.type; + vectorcp(offset,hose->end.offset); + if (hose->end.attrib != 0) // FIXED size end, do not stretch it. + matrixcp(m2,hose->end.orient); + else // Stretch it. + matrixmult3(m2,hose->end.orient,m1); + } else if ((i & 0x01) && (strlen(hose->alt.type) != 0)) { + if (i == 1) printf("ALT = %s\n", hose->alt.type); + type = hose->alt.type; + vectorcp(offset,hose->alt.offset); + matrixmult3(m2,hose->alt.orient,m1); + } else { + type = hose->mid.type; + vectorcp(offset,hose->mid.offset); + matrixmult3(m2,hose->mid.orient,m1); + } + + /* + * twist helps with string or chains + */ + + m1[0][0] = 1; + m1[0][1] = 0; + m1[0][2] = 0; + m1[1][0] = 0; + m1[1][1] = 1; + m1[1][2] = 0; + m1[2][0] = 0; + m1[2][1] = 0; + m1[2][2] = 1; + + if (hose->fill != STRETCH) { + PRECISION angle; + *total_twist += hose->twist; // One twist before calculating angle. +#define ORIENT_FN_FIXED_FOR_XZ_AND_YZ_CURVES +#ifdef ORIENT_FN_FIXED_FOR_XZ_AND_YZ_CURVES + // For N FIXED segments start with a full twist (not a half twist). + if (hose->fill > FIXED) { + angle = *total_twist * pi / 180; // Calculate angle after the twist. + }else +#endif + { + angle = *total_twist * pi / 360; // Not (pi/180) because we double the twist. + *total_twist += hose->twist; // Another twist after calculating angle. + // NOTE: + // Breaking the twist up this way draws the Minifig chain with + // the links twisted 45 degrees from the start post (instead of 90). + // This looks OK because the real chains sometimes rest that way. + // It also avoids the problem with the orient() fn. + // Chains which turn in the XZ or YZ planes may rotate 45 degrees + // the other way because orient() only works in the XY plane. + // But that's better than the 90 degrees wrong without this hack. + } + m1[0][0] = cos(angle); + m1[0][2] = sin(angle); + m1[2][0] = -sin(angle); + m1[2][2] = cos(angle); + } + matrixmult(m1,m2); + matrixmult3(m2,segments[i].orient,m1); + + // NOTE: We handle hose->(start,mid,end).orient here, but we do nothing + // with the hose->(start,mid,end).offset. + // I really think we need to use this to fix the minifig chain link, the + // origin of which is not centered. Instead its ~4Y over toward one end. +#if 0 + offset[0] = 0; offset[1] = 0; offset[2] = 0; + vectoradd(offset,hose->mid.offset); // Should also consider first and last. +#else + // Get offset (first, mid, or last) from lsynth.mpd file above. + // That way it matches the first, mid, or last orient from lsynth.mpd. +#endif + vectorrot(offset,m2); + vectoradd(segments[i].offset,offset); + + // FIXME: I would expect to have to flip the array along the diagonal + // like we have to do in input. + + output_line(output,ghost,group,color, + segments[i].offset[0], segments[i].offset[1], segments[i].offset[2], + m2[0][0], m2[0][1], m2[0][2], + m2[1][0], m2[1][1], m2[1][2], + m2[2][0], m2[2][1], m2[2][2], + type); + if (group) { + gs++; + } + } + *group_size = gs; +} + +void +adjust_constraint( + part_t *part, + part_t *orig, + int last) +{ + int i; + PRECISION m[3][3]; + + *part = *orig; + + for (i = 0; i < N_HOSE_CONSTRAINTS; i++) { + if (strcasecmp(part->type,hose_constraints[i].type) == 0) { + + // adjust the constraints offset via hose_constraint + vectorcp(part->offset,hose_constraints[i].offset); + vectorrot(part->offset,hose_constraints[i].orient); + vectoradd(part->offset,orig->offset); + + // compensate part orient via hose_constraint + matrixcp(part->orient,hose_constraints[i].orient); + matrixmult(part->orient,orig->orient); + + if (hose_constraints[i].attrib && last) { + matrixcp(m,part->orient); + matrixneg(part->orient,m); + } + break; + } + } + return; +} + +/* + * a 1x1 brick is 20 LDU wide and 24 LDU high + * + * hose length = 14 brick widths long = 280 LDU + * number of ribs = 45 + * 6.2 LDU per rib + * + */ + +void +render_hose( + hose_attrib_t *hose, + int n_constraints, + part_t *constraints, + PRECISION bend_res, + PRECISION twist_res, + int ghost, + char *group, + int group_size, + int color, + FILE *output) +{ + int c, n_segments; + part_t mid_constraint; + PRECISION total_twist = 0; + + if ( ! ldraw_part) { + fprintf(output,"0 SYNTH SYNTHESIZED BEGIN\n"); + } + + // First and Last parts for STRETCH hose could be FIXED length. + // Add up to two new constraints to handle this. They're not used afterwards. + // Just don't go over 128 constraints. + if (hose->fill == STRETCH) { + PRECISION offset[3]; + PRECISION l; + +#ifdef DEBUGGING_HOSES + printf("STRETCH = (%d, %d, %d)\n", + hose->start.attrib, hose->mid.attrib, hose->end.attrib); +#endif + + l = hose->start.attrib; + if (l != 0.0) { + n_constraints++; + for (c = n_constraints-1; c > 0; c--) { + memcpy(&constraints[c], &constraints[c-1], sizeof(part_t)); + } + + offset[0] = 0; offset[1] = -l; offset[2] = 0; + vectorrot(offset,constraints[0].orient); + vectoradd(constraints[1].offset, offset); + } + + l = hose->end.attrib; + if (l != 0.0){ + n_constraints++; + c = n_constraints-1; + memcpy(&constraints[c], &constraints[c-1], sizeof(part_t)); + + offset[0] = 0; offset[1] = l; offset[2] = 0; + vectorrot(offset,constraints[n_constraints-1].orient); + vectoradd(constraints[n_constraints-2].offset, offset); + } + } + + mid_constraint = constraints[0]; + + for (c = 0; c < n_constraints - 1; c++) { + part_t first,second; + + // reorient imperfectly oriented or displaced constraint types + + adjust_constraint(&first,&mid_constraint,0); + + // reorient imperflectly oriented or displaced constraint types + + adjust_constraint(&second, &constraints[c+1],c == n_constraints-2); + + n_segments = MAX_SEGMENTS; + + // For N FIXED segments break up segments[MAX_SEGMENTS] into chunks. + if (hose->fill > FIXED) + { + // Break up seglist into fixed size chunks based on number of constraints. + // We could do better by considering the actual length of each chunk... + n_segments = MAX_SEGMENTS / (n_constraints - 1); + //if (c == 0) printf("FIXED%d, N_constraints = %d, chunksize = %d\n", hose->fill, n_constraints, n_segments); + } + + // create an oversampled curve + + if (hose->fill == FIXED) // Save room for end constraint point. + synth_curve(&first,&second,segments,n_segments-1,hose->stiffness,output); + else if (hose->fill == STRETCH) + synth_curve(&first,&second,segments,n_segments-1,hose->stiffness,output); + else if (hose->fill > FIXED) + synth_curve(&first,&second,segments,n_segments-1,hose->stiffness,output); + else // Old way. Overwrite last point with end constraint point. Not good. + synth_curve(&first,&second,segments,n_segments,hose->stiffness,output); + + // reduce oversampled curve to fixed length chunks, or segments limit + // by angular resolution + + if (hose->fill == STRETCH) { + // Make sure final segment matches second constraint + vectorcp(segments[n_segments-1].offset,second.offset); + merge_segments_angular( + &first, + &second, + segments, + &n_segments, + bend_res, + twist_res, + output); + // Make sure final segment matches second constraint + vectorcp(segments[n_segments-1].offset,second.offset); + // move normalized result back into its original orientation and position + mid_constraint = constraints[c+1]; +#ifdef DEBUGGING_HOSES + printf("orient(N_SEGMENTS = %d)\n", n_segments); +#endif + orientq(&first,&second,n_segments,segments); // With quaternions! + } + else if (hose->fill == FIXED) { + // Make sure final segment point matches second constraint + vectorcp(segments[n_segments-1].offset,second.offset); +#ifdef ADJUST_FINAL_FIXED_HOSE_END + // Hmmm, how do we make a hose comprised of fixed length parts + // reach exactly to the end constraint? + // This is OK for ribbed hoses + // but not for string or chain, so we probably shouldn't do it. + if (c == n_constraints-2) + { + int i = n_segments; + memcpy(seglist, segments, n_segments*sizeof(part_t)); + // Set i to how many segments we need to get near to the end. + merge_segments_length(seglist,&i,hose->mid.attrib,output); + // Squish an extra part into the last segment to make it reach the end. + merge_segments_count(hose,&first,&second,segments,&n_segments,i,output); // Yuck! + // Or, stretch the last part of the hose a bit to make it reach the end. + // merge_segments_count(hose, segments,&n_segments,i-1,output); // Yuckier! + } + else +#endif + merge_segments_length(segments,&n_segments,hose->mid.attrib,output); + // move normalized result back into its original orientation and position + mid_constraint = constraints[c+1]; + vectorcp(mid_constraint.offset,segments[n_segments-1].offset); + orient(&first,&second,n_segments,segments); + //orientq(&first,&second,n_segments,segments); + } + else { // For N fixed size chunks just copy into one big list, merge later. + // Make sure final segment matches second constraint + vectorcp(segments[n_segments-1].offset,second.offset); + // move normalized result back into its original orientation and position + mid_constraint = constraints[c+1]; + vectorcp(mid_constraint.offset,segments[n_segments-2].offset); + memcpy(seglist+(n_segments*c), segments, n_segments*sizeof(part_t)); + } + + // output the result (if not FIXED number of segments) + if (hose->fill <= FIXED) + render_hose_segment( + hose, + ghost, + group, + &group_size, + color, + segments,n_segments, + &total_twist, + c ==0, + c == n_constraints-2, + output, + &constraints[0]); + } + + // output the result (if FIXED number of segments) + if (hose->fill > FIXED) { + part_t first,second; + + // Reorient imperfectly oriented or displaced constraint types. + // NOTE: I really need to study the new orient code and see why it + // does not seem to work until after merge_segment_count() below. + // It needs to orient based on ALL constraints, not just first and last. + adjust_constraint(&first,&constraints[0],0); + adjust_constraint(&second, &constraints[n_constraints-1],1); + + n_segments *= c; + // Make sure final segment matches second constraint + vectorcp(segments[n_segments-1].offset,second.offset); + merge_segments_count(hose,&first,&second,seglist,&n_segments,hose->fill,output); + //printf("Merged segments to %d segments of len %d\n", n_segments, hose->mid.attrib); + //orient(&first,&second,n_segments,seglist); + orientq(&first,&second,n_segments,seglist); // With quaternions! + //printf("oriented %d\n",n_segments); + + render_hose_segment( + hose, + ghost, + group, + &group_size, + color, + seglist,n_segments, + &total_twist, + 1, // First AND + 1, // Last part of the hose. (seglist = one piece hose) + output, + &constraints[0]); + //printf("Total twist = %.1f (%.1f * %.1f) n = %d\n", total_twist, hose->twist, total_twist/hose->twist, n_segments); + } + + if (group) { + fprintf(output,"0 GROUP %d %s\n",group_size,group); + } + + if ( ! ldraw_part) { + fprintf(output,"0 SYNTH SYNTHESIZED END\n"); + } +} + +int +synth_hose( + char *type, + int n_constraints, + part_t *constraints, + int ghost, + char *group, + int group_size, + int color, + FILE *output) +{ + int i; + + for (i = 0; i < N_HOSE_TYPES; i++) { + if (strcasecmp(hose_types[i].type,type) == 0) { + render_hose( + &hose_types[i], + n_constraints,constraints, + max_bend, + max_twist, + ghost, + group, + group_size, + color, + output); + return 0; + } + } + return 1; +} + diff --git a/lsynth/src/hose.h b/lsynth/src/hose.h new file mode 100644 index 000000000..ed2758fe8 --- /dev/null +++ b/lsynth/src/hose.h @@ -0,0 +1,50 @@ +/* + * This file describes the interface to the LDRAW synthesizable parts library. + * Kevin Clague + */ +#ifndef TUBE_H +#define TUBE_H + +#ifdef _cplusplus +extern "C" { +#endif + +typedef struct { + char type[256]; + char descr[256]; + int fill; // FIXED or STRETCH + PRECISION diameter; // of cross section + PRECISION stiffness; // stiffness + PRECISION twist; // in degrees + part_t start; // LDraw part for start of hose + part_t mid; // LDraw part for mid section + part_t end; // LDraw part for end of hose + part_t alt; // LDraw part alternate for mid of hose +} hose_attrib_t; + +extern hose_attrib_t hose_types[]; +extern int n_hose_types; +extern part_t hose_constraints[]; +extern int n_hose_constraints; + +void list_hose_types( void); +void list_hose_constraints(void); +void hose_ini( void); +int ishosetype( char *type); +int ishoseconstraint(char *type); + +int +synth_hose( + char *type, + int n_constraints, + part_t *constraints, + int ghost, + char *group, + int group_size, + int color, + FILE *output); +#ifdef _cplusplus +}; +#endif + +#endif diff --git a/lsynth/src/lsynthcp.c b/lsynth/src/lsynthcp.c new file mode 100644 index 000000000..ca1b72da6 --- /dev/null +++ b/lsynth/src/lsynthcp.c @@ -0,0 +1,2084 @@ +//--------------------------------------------------------------------------- + +#pragma hdrstop + +/* + * Program Name: LSynth + * Program Description: + * LDraw CAD system compatible flexible part synthesizer. This program + * reads your LDraw file, searching for unofficial META commands (structured + * comments), that specify things you want synthesized. + * + * LSynth has two primary forms of synthesis. The first kind of synthesis + * creates hose like things including: + * rubbed, rubber, pneumatic, and flex system hoses, as well as electric + * and fiber optic cables, flex system cables, flexible axles, string, and + * minifig chain. + * + * It also creates things that travel around circular lego parts. Things + * like: + * rubber band, rubber belt, technic chain, technic plastic tread, technic + * rubber tread. + * + * The files tube.c, tube.h, curve.c and curve.h perform hose synthesis. + * The files band.c and band.h perform band synthesis. + * + * This file (main.c) contains the main entry/exit points for the program. + * It opens and scans the LDraw file provided, identifies synthesis + * synthesis specifications and hands them off to the appropriate synthesis + * methodology. + */ + +#pragma hdrstop + +#ifdef WIN32 +// Added a messagebox for lsynth.mpd version mismatch. +#include +#endif + +#include + +#include "lsynthcp.h" +#include "hose.h" +#include "band.h" + +typedef struct { + char name[126]; + char nickname[128]; + char method[128]; +} product_t; + +product_t products[256]; +int n_products = 0; + +char version[] = "3.1"; +char beta[] = ""; // " Beta I"; + +char mpdversion[32] = "UNKNOWN"; + +int ldraw_part = 0; +int group_size; + +//--------------------------------------------------------------------------- +void messagebox( const char* title, const char* message ) +{ + char cmd[1024]; + int i; + +#if defined( __APPLE__ ) && defined( __MACH__ ) + \\ Skip activate if we dont want to focus on dialog. Finder will bounce instead. + sprintf(cmd, "osascript -e \'tell app \"Finder\" to activate display dialog \"%s\"\'", message); + i = system(cmd); + +#elif WIN32 + MessageBox( NULL, message, title, MB_OK ); + +#else + // Unknown OS. We'll probably have to settle for commandline warnings. + // But first attempt to launch an oldsKool stylee xmessage. + // Gnome systems often come with the much prettier zenity. Try it first. + sprintf(cmd, "zenity --warning --title=\"%s\" --text=\"%s\" 2>1 >/dev/null", title, message); + i = system(cmd); + i = i >> 8; // Get exit value of zenity. + if ((i == 0) || (i == 1)) // (0=OK, 1=X) + return; + + // No gnome? Try kdialog from KDE. + sprintf(cmd, "kdialog --title=\"%s\" --sorry \"%s\" 2>1 >/dev/null", title, message); + i = system(cmd); + i = i >> 8; // Get exit value of kdialog. + if ((i == 0) || (i == 1)) // (0=OK, 1=X) + return; + + // The unpatched Athena widgets are hideous, but there's a pretty good chance it'll work. + sprintf(cmd, "xmessage -bg lightgrey -fn 9x15bold -buttons OK -center -title \"%s\" \"%s\"", title, message); + // I should probably skip the fork and just use system to make it a modal messagebox. + if(fork()==0){ + close(1); close(2); + system(cmd); + exit(0); + } + + // Still here? Sorry, yer stuck with command line output. +#endif +} + +//--------------------------------------------------------------------------- + /* If this code works, it was written by Lars C. Hassing. */ + /* If not, I don't know who wrote it. */ + + /* Like fgets, except that 1) any line ending is accepted (\n (unix), + \r\n (DOS/Windows), \r (Mac (OS9)) and 2) Str is ALWAYS zero terminated + (even if no line ending was found) */ + //--------------------------------------------------------------------------- + char *L3fgets(char *Str, int n, FILE *fp) + { + register int c; + int nextc; + register char *s = Str; + + while (--n > 0) + { + if ((c = getc(fp)) == EOF) + break; + if (c == '\032') + continue; /* Skip CTRL+Z */ + if (c == '\r' || c == '\n') + { + *s++ = '\n'; + /* We got CR or LF, eat next character if LF or CR respectively */ + if ((nextc = getc(fp)) == EOF) + break; + if (nextc == c || (nextc != '\r' && nextc != '\n')) + ungetc(nextc, fp); /* CR-CR or LF-LF or ordinary character */ + break; + } + *s++ = c; + } + *s = 0; + + /* if (ferror(fp)) return NULL; if (s == Str) return NULL; */ + if (s == Str) + return NULL; + + return Str; + } + + //--------------------------------------------------------------------------- + +char * +fgetline( + char *line, + int len, + FILE *file) +{ + char *rc; + while ((rc = L3fgets(line,len,file))) { + char *nonwhite; + + nonwhite = line + strspn(line," \t"); + + if (strncasecmp(nonwhite,"0 ROTATION C",strlen("0 ROTATION C")) == 0 || + strncasecmp(nonwhite,"0 COLOR",strlen("0 COLOR")) == 0) { + continue; + } + + nonwhite = line + strspn(line," \t"); + if (strncasecmp(nonwhite,"0 WRITE ",strlen("0 WRITE ")) == 0) { + strcpy(nonwhite + 2, nonwhite + strlen("0 WRITE ")); + } + break; + } + return rc; +} + +void +strclean(char *str) +{ + if (strncasecmp(str,"0 WRITE ",strlen("0 WRITE ")) == 0) { + strcpy(str + 2, str + strlen("0 WRITE ")); + } +} + +//--------------------------------------------------------------------------- + +/* + * Skip over MLCad ROTATION and COLOR statements + */ + +static int skip_rot(char *line, int n_line, FILE *dat, FILE *temp) +{ + char *nonwhite; + + nonwhite = line + strspn(line,"\t"); + + while (strncasecmp(nonwhite,"0 ROTATION C",strlen("0 ROTATION C")) == 0 || + strncasecmp(nonwhite,"0 COLOR",strlen("0 COLOR")) == 0) { + fputs(line,temp); + L3fgets(line,n_line,dat); /* FIXME: check fgets rc */ + nonwhite = line + strspn(line," \t"); + } + + return 0; +} + + +/***************************************************************************** + * + * Read in and parse up synthesis descriptions and constraints. + * + ****************************************************************************/ + +int +parse_descr(char *fullpath_progname) +{ + char filename[256]; + FILE *mpd; + char line[256]; + + strcpy(filename,fullpath_progname); + { + char *l, *p; + + for (l = p = filename; *p; *p++) { + if (*p == '\\' || *p == '/') { + l = p+1; + } + } + *l = '\0'; + } + strcat(filename,"lsynth.mpd"); + + mpd = fopen(filename,"r"); + + if (mpd == NULL) { + printf("Failed to open lsynth.mpd for reading.\n"); + messagebox("LSynth", "Failed to open lsynth.mpd for reading."); + return -1; + } + + while(fgetline(line,sizeof(line),mpd)) { + char stretch[64]; + char type[64]; + char product[126], nickname[128], method[128]; + int d,st,i; + PRECISION s,t; + int got_end = 0; + + strclean(line); + + if (sscanf(line,"0 !VERSION %d.%d\n", &i, &d) == 2) { + sprintf(mpdversion, "%d.%d", i, d); + } + else if (!strncmp(line,"0 SYNTH ", 8)) + { + if (strcmp(mpdversion, version)) + { + char s[256]; + sprintf(s, "\nWarning: lsynth.mpd version %s does not match executable version %s!", + mpdversion, version); + printf("%s\n\n", s); + messagebox("LSynth", s); + strcpy(mpdversion, version); // Only warn once. + } + } + + if (sscanf(line,"0 SYNTH PART %s %s %s\n",product, nickname, method) == 3) { + strcpy(products[n_products].name,product); + strcpy(products[n_products].nickname,nickname); + strcpy(products[n_products].method,method); + n_products++; + } else if (sscanf(line,"0 SYNTH BEGIN DEFINE %s HOSE %s %d %d %f\n", + type,stretch,&d,&st,&t) == 5) { + if (strcasecmp(stretch,"STRETCH") == 0) { + hose_types[n_hose_types].fill = STRETCH; + } else if (strcasecmp(stretch,"FIXED") == 0) { + hose_types[n_hose_types].fill = FIXED; + } else if ((strncasecmp(stretch,"FIXED",strlen("FIXED")) == 0) && + (sscanf(stretch, "FIXED%d", &i) == 1) && (i >1)) { + hose_types[n_hose_types].fill = i; + } else { + printf("Error: Unrecognized fill type %s for hose type %s. Aborting\n", + stretch,type); + fclose(mpd); + return -1; + } + + strcpy(hose_types[n_hose_types].type,type); + hose_types[n_hose_types].diameter = d; + hose_types[n_hose_types].stiffness = st; + hose_types[n_hose_types].twist = t; + + for (i = 0; i < 3; i++) { + part_t *part; + int got_part = 0; + + if (i == 0) { + part = &hose_types[n_hose_types].start; + } else if (i == 1) { + part = &hose_types[n_hose_types].mid; + } else { + part = &hose_types[n_hose_types].end; + } + + while (fgetline(line,sizeof(line),mpd)) { + + int n; + + n = sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type); + + if (n == 14) { + got_part = 1; + break; + } + } + if ( ! got_part) { + printf("Error: Unexpected end of file\n"); + fclose(mpd); + return -1; + } + } + + // Assume no alternate mid part. + strcpy(hose_types[n_hose_types].alt.type, ""); + + got_end = 0; + while (fgetline(line,sizeof(line),mpd)) { + part_t *part; + int n; + + if (strcasecmp(line,"0 SYNTH END\n") == 0) { + got_end = 1; + break; + } + + // Look for an alternate mid part + part = &hose_types[n_hose_types].alt; + + n = sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type); + + if (n != 14) + strcpy(hose_types[n_hose_types].alt.type, ""); // Skip comments + else + { +#ifdef DEBUGGING_HOSES + printf("Found HOSE alt segment %s\n", hose_types[n_hose_types].alt.type); +#endif + } + } + if ( ! got_end) { + printf("Error: Unexepcted end of file\n"); + fclose(mpd); + return -1; + } + n_hose_types++; + } else if (strcasecmp(line,"0 SYNTH BEGIN DEFINE HOSE CONSTRAINTS\n") == 0) { + while(fgetline(line,sizeof(line),mpd)) { + part_t *part = &hose_constraints[n_hose_constraints]; + if (sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type) == 14) { + n_hose_constraints++; + } else if (strcasecmp(line,"0 SYNTH END\n") == 0) { + break; + } + } + } else if (sscanf(line,"0 SYNTH BEGIN DEFINE %s BAND %s %f %f\n", + type,stretch,&s,&t) == 4) { + int n; + + if (strcasecmp(stretch,"STRETCH") == 0) { + band_types[n_band_types].fill = STRETCH; + n = 2; + } else if (strcasecmp(stretch,"FIXED") == 0) { + band_types[n_band_types].fill = FIXED; + n = 2; + } else if (strcasecmp(stretch,"FIXED3") == 0) { + band_types[n_band_types].fill = FIXED3; + n = 4; + } else { + printf("Error: Unrecognized fill type %s for hose type %s. Aborting\n", + stretch,type); + fclose(mpd); + return -1; + } + + strcpy(band_types[n_band_types].type,type); + band_types[n_band_types].scale = s; + band_types[n_band_types].thresh = t; + + for (i = 0; i < n; i++) { + part_t *part; + int got_part = 0; + + if (i == 0) { + part = &band_types[n_band_types].tangent; + } else if (i == 1) { + part = &band_types[n_band_types].arc; + } else if (i == 2) { + part = &band_types[n_band_types].start_trans; + } else { + part = &band_types[n_band_types].end_trans; + } + + while (fgetline(line,sizeof(line),mpd)) { + if (sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type) == 14) { + got_part = 1; + break; + } + } + if ( ! got_part) { + printf("Error: Unexpected end of file\n"); + fclose(mpd); + return -1; + } + } + + if (L3fgets(line,sizeof(line),mpd)) { + if (strcasecmp(line,"0 SYNTH END\n") != 0) { + printf("Error: Expected SYNTH END, got this instead\n"); + printf(line); + fclose(mpd); + return -1; + } + } else { + printf("Error: Unexepcted end of file\n"); + fclose(mpd); + return -1; + } + n_band_types++; + } else if (sscanf(line,"0 SYNTH BEGIN DEFINE %s BAND %s %f %f\n", + type,stretch,&s,&t) == 4) { + int n; + + if (strcasecmp(stretch,"STRETCH") == 0) { + band_types[n_band_types].fill = STRETCH; + n = 2; + } else if (strcasecmp(stretch,"FIXED") == 0) { + band_types[n_band_types].fill = FIXED; + n = 2; + } else if (strcasecmp(stretch,"FIXED3") == 0) { + band_types[n_band_types].fill = FIXED3; + n = 4; + } else { + printf("Error: Unrecognized fill type %s for hose type %s. Aborting\n", + stretch,type); + fclose(mpd); + return -1; + } + + strcpy(band_types[n_band_types].type,type); + band_types[n_band_types].scale = s; + band_types[n_band_types].thresh = t; + band_types[n_band_types].pulley = 0; + + for (i = 0; i < n; i++) { + part_t *part; + int got_part = 0; + + if (i == 0) { + part = &band_types[n_band_types].tangent; + } else if (i == 1) { + part = &band_types[n_band_types].arc; + } else if (i == 2) { + part = &band_types[n_band_types].start_trans; + } else { + part = &band_types[n_band_types].end_trans; + } + + while(fgetline(line,sizeof(line),mpd)) { + if (sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type) == 14) { + got_part = 1; + break; + } + } + if ( ! got_part) { + printf("Error: Unexpected end of file\n"); + fclose(mpd); + return -1; + } + } + + if (L3fgets(line,sizeof(line),mpd)) { + if (strcasecmp(line,"0 SYNTH END\n") != 0) { + printf("Error: Expected SYNTH END, got this instead\n"); + printf(line); + fclose(mpd); + return -1; + } + } else { + printf("Error: Unexepcted end of file\n"); + fclose(mpd); + return -1; + } + n_band_types++; + } else if (sscanf(line,"0 SYNTH BEGIN DEFINE %s PULLEY %s %f %f\n", + type,stretch,&s,&t) == 4) { + int n; + + if (strcasecmp(stretch,"STRETCH") == 0) { + band_types[n_band_types].fill = STRETCH; + n = 2; + } else if (strcasecmp(stretch,"FIXED") == 0) { + band_types[n_band_types].fill = FIXED; + n = 2; + } else if (strcasecmp(stretch,"FIXED3") == 0) { + band_types[n_band_types].fill = FIXED3; + n = 4; + } else { + printf("Error: Unrecognized fill type %s for hose type %s. Aborting\n", + stretch,type); + fclose(mpd); + return -1; + } + + strcpy(band_types[n_band_types].type,type); + band_types[n_band_types].scale = s; + band_types[n_band_types].thresh = t; + + band_types[n_band_types].pulley = 1; + + for (i = 0; i < n; i++) { + part_t *part; + int got_part = 0; + + if (i == 0) { + part = &band_types[n_band_types].tangent; + } else if (i == 1) { + part = &band_types[n_band_types].arc; + } else if (i == 2) { + part = &band_types[n_band_types].start_trans; + } else { + part = &band_types[n_band_types].end_trans; + } + + while (fgetline(line,sizeof(line),mpd)) { + if (sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type) != 14) { + got_part = 1; + break; + } + } + if ( ! got_part) { + printf("Error: Unexpected end of file\n"); + fclose(mpd); + return -1; + } + } + + if (L3fgets(line,sizeof(line),mpd)) { + if (strcasecmp(line,"0 SYNTH END\n") != 0) { + printf("Error: Expected SYNTH END, got this instead\n"); + printf(line); + fclose(mpd); + return -1; + } + } else { + printf("Error: Unexepcted end of file\n"); + fclose(mpd); + return -1; + } + n_band_types++; + + } else if (strcasecmp(line,"0 SYNTH BEGIN DEFINE BAND CONSTRAINTS\n") == 0) { + while(fgetline(line,sizeof(line),mpd)) { + part_t *part = &band_constraints[n_band_constraints]; + if (sscanf(line,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s\n", + &part->attrib, + &part->offset[0], &part->offset[1], &part->offset[2], + &part->orient[0][0], &part->orient[0][1], &part->orient[0][2], + &part->orient[1][0], &part->orient[1][1], &part->orient[1][2], + &part->orient[2][0], &part->orient[2][1], &part->orient[2][2], + part->type) == 14) { + n_band_constraints++; + } else if (strcasecmp(line,"0 SYNTH END\n") == 0) { + break; + } + } + } + } + + fclose(mpd); + return 0; +} + +void list_products() +{ + int i; + + printf("\n\nComplete parts LSynth can create\n"); + for (i = 0; i < n_products; i++) { + printf(" %s %s (%s)\n", + products[i].name, + products[i].nickname, + products[i].method); + } +} + +void product_ini(void) +{ + int i; + + for (i = 0; i < n_products; i++) { + printf("%-20s = SYNTH BEGIN %s 16\n", + products[i].nickname, + products[i].nickname); + } + + for (i = 0; i < n_products; i++) { + printf("%-20s = SYNTH BEGIN %s 16\n", + products[i].name, + products[i].name); + } +} + +char * +isproduct(char *type) +{ + int i; + + for (i = 0; i < n_products; i++) { + if (strncasecmp(products[i].name,type,strlen(products[i].name)) == 0) { + return products[i].name; + } + if (strncasecmp(products[i].nickname,type,strlen(products[i].nickname)) == 0) { + return products[i].name; + } + } + return NULL; +} + +char * +product_method(char *type) +{ + int i; + + for (i = 0; i < n_products; i++) { + if (strncasecmp(products[i].name,type,strlen(products[i].name)) == 0) { + return products[i].method; + } + if (strncasecmp(products[i].nickname,type,strlen(products[i].nickname)) == 0) { + return products[i].method; + } + } + return NULL; +} + +char * +product_nickname(char *type) +{ + int i; + + for (i = 0; i < n_products; i++) { + if (strncasecmp(products[i].name,type,strlen(products[i].name)) == 0) { + return products[i].nickname; + } + if (strncasecmp(products[i].nickname,type,strlen(products[i].nickname)) == 0) { + return products[i].nickname; + } + } + return NULL; +} + + +/* + * Skip over results rom previous syntesis efforts. + */ + +int skip_synthesized(FILE *dat, char *line, int sizeof_line) +{ + int rc; + char *nonwhite; + + while (L3fgets(line,sizeof(line),dat)) { + nonwhite = line + strspn(line," \t"); + if (strcasecmp(nonwhite,"0 SYNTHESIZED END\n") == 0) { + return 0; + } + } + return -1; +} + +/* + * Gather constraints from hose synthesis + */ + +static +int synth_hose_class( + char *method, + int hose_color, + FILE *dat, + FILE *temp, + char *group) +{ + char line[512]; + char *nonwhite; + part_t constraints[128]; + int constraint_n = 0; + int color; + char start_type[64]; + float x,y,z, a,b,c, d,e,f, g,h,i; + int rc = 0; + int hide = 1; + int ghost = 0; + int group_count = 0; + char group_name[512]; + + if (group) { + strcpy(group_name,group); + group = group_name; + } else { + group_name[0] = '\0'; + } + + memset(constraints, 0, 128*sizeof(part_t)); + + /* gather up the constraints */ + + while (L3fgets(line,sizeof(line), dat)) { + nonwhite = line + strspn(line," \t"); + + skip_rot(line,sizeof(line),dat,temp); + + nonwhite = line + strspn(line," \t"); + + if (strncasecmp(nonwhite,"0 GHOST ",strlen("0 GHOST ")) == 0) { + ghost = 1; + nonwhite += strlen("0 GHOST "); + + nonwhite += strspn(nonwhite," \t"); + } + + strclean(nonwhite); + + if (sscanf(nonwhite,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s", + &color, &x,&y,&z, &a,&b,&c, &d,&e,&f, &g,&h,&i, start_type) == 14) { + + if (ishoseconstraint(start_type)) { + part_t *constr = &constraints[constraint_n]; + + if ( ! ldraw_part) { + if (hide) { + fputs("0 ",temp); + } + fputs(line,temp); + } + constr->offset[0] = x; constr->offset[1] = y; constr->offset[2] = z; + + constr->orient[0][0] = a;constr->orient[0][1] = b;constr->orient[0][2] = c; + constr->orient[1][0] = d;constr->orient[1][1] = e;constr->orient[1][2] = f; + constr->orient[2][0] = g;constr->orient[2][1] = h;constr->orient[2][2] = i; + + strcpy(constraints[constraint_n].type,start_type); + constraint_n++; + } else { + if (group) { + fprintf(temp,"0 MLCAD BTG %s\n",group); + group_count++; + } + fputs(line,temp); + } + } else if (strcasecmp(nonwhite,"0 SYNTH HIDE\n") == 0) { + fputs(line,temp); + hide = 1; + } else if (strcasecmp(nonwhite,"0 SYNTH SHOW\n") == 0) { + fputs(line,temp); + hide = 0; + } else if (strncasecmp(nonwhite,"0 MLCAD BTG ",strlen("0 MLCAD BTG ")) == 0) { + if (! group) { + nonwhite += strlen("0 MLCAD BTG "); + strcpy(group_name,nonwhite); + group = group_name; + group[strlen(group)-1] = '\0'; + } + } else if (strncasecmp(nonwhite,"0 GROUP ",strlen("0 GROUP ")) == 0) { + fputs(line,temp); + } else if (strcasecmp(line,"0 SYNTHESIZED BEGIN\n") == 0) { + rc = skip_synthesized(dat, line, sizeof(line)); + if (rc < 0) { + break; + } + } else if (strncasecmp(nonwhite,"0 SYNTH END\n",strlen("0 SYNTH END\n")) == 0 || + strncasecmp(nonwhite,"0 WRITE SYNTH END\n",strlen("0 WRITE SYNTH END\n")) == 0) { + rc = synth_hose(method,constraint_n,constraints,ghost,group,group_count,hose_color,temp); + if ( ! ldraw_part ) { + fputs(line,temp); + } + break; + } else { + fputs(line,temp); + } + } + return rc; +} + +/* + * Gather up rubber band constraints + */ + +static +int synth_band_class( + char *method, + int color, + FILE *dat, + FILE *temp, + char *group) +{ + char line[512]; + char *nonwhite; + LSL_band_constraint constraints[128]; + int constraint_n = 0; + int rc = 0; + int hide = 0; + int ghost = 0; + + memset(constraints, 0, sizeof(LSL_band_constraint)*128); + + /* gather up the constraints */ + + while (L3fgets(line,sizeof(line), dat)) { + float x,y,z, a,b,c, d,e,f, g,h,i; + char start_type[64]; + int t; + + nonwhite = line + strspn(line, " \t"); + + skip_rot(line,sizeof(line),dat,temp); + + nonwhite = line + strspn(line, " \t"); + + if (strncasecmp(nonwhite,"0 GHOST ",strlen("0 GHOST ")) == 0) { + ghost = 1; + nonwhite += strlen("0 GHOST "); + + nonwhite += strspn(nonwhite," \t"); + } + + if (sscanf(nonwhite,"1 %d %f %f %f %f %f %f %f %f %f %f %f %f %s", + &t, &x,&y,&z, &a,&b,&c, &d,&e,&f, &g,&h,&i, start_type) == 14) { + + if (isbandconstraint(start_type)) { + part_t *cp = &constraints[constraint_n].part; + + if (hide) { + fputs("0 ",temp); + } + fputs(line,temp); + + constraints[constraint_n].part.attrib = color; + strcpy(constraints[constraint_n].part.type,start_type); + + cp->offset[0] = x; cp->offset[1] = y; cp->offset[2] = z; + cp->orient[0][0] = a;cp->orient[0][1] = b;cp->orient[0][2] = c; + cp->orient[1][0] = d;cp->orient[1][1] = e;cp->orient[1][2] = f; + cp->orient[2][0] = g;cp->orient[2][1] = h;cp->orient[2][2] = i; + + strcpy(constraints[constraint_n].part.type,start_type); + constraint_n++; + } + } else { + + strclean(nonwhite); + + if (strcasecmp(nonwhite,"0 SYNTH INSIDE\n") == 0) { + fputs(line,temp); + strcpy(constraints[constraint_n].part.type,"INSIDE"); + constraint_n++; + } else if (strcasecmp(nonwhite,"0 SYNTH OUTSIDE\n") == 0) { + fputs(line,temp); + strcpy(constraints[constraint_n].part.type,"OUTSIDE"); + constraint_n++; + } else if (strcasecmp(nonwhite,"0 SYNTH CROSS\n") == 0) { + fputs(line,temp); + strcpy(constraints[constraint_n].part.type,"CROSS"); + constraint_n++; + } else if (strcasecmp(nonwhite,"0 SYNTH HIDE\n") == 0) { + fputs(line,temp); + hide = 1; + } else if (strcasecmp(nonwhite,"0 SYNTH SHOW\n") == 0) { + fputs(line,temp); + hide = 0; + } else if (strncasecmp(nonwhite,"0 MLCAD BTG ",strlen("0 MLCAD BTG ")) == 0) { + if (! group) { + group = nonwhite + strlen("0 MLCAD BTG "); + group[strlen(group)-1] = '\0'; + } + fputs(line,temp); + } else if (strncasecmp(nonwhite,"0 GROUP ",strlen("0 GROUP ")) == 0) { + } else if (strcasecmp(line,"0 SYNTHESIZED BEGIN\n") == 0) { + rc = skip_synthesized(dat, line, sizeof(line)); + if (rc < 0) { + break; + } + } else if (strncasecmp(nonwhite,"0 SYNTH END\n",strlen("0 SYNTH END\n")) == 0 || + strncasecmp(nonwhite,"0 WRITE SYNTH END\n",strlen("0 WRITE SYNTH END\n")) == 0) { + + rc = synth_band(method,constraint_n,constraints,color,temp,ghost,group); + if ( ! ldraw_part ) { + fputs(line,temp); + } + break; + } else { + fputs(line,temp); + } + } + } + return 0; +} + +PRECISION max_bend = 0.05; +PRECISION max_twist = 0.0174; +PRECISION band_res = 1; + +//--------------------------------------------------------------------------- + +char * stripquotes(char *s) +{ + char *p; + int i; + + // Strip away leading whitespace (spaces and tabs). + s += strspn(s, " \t"); + + // Remove leading quotes + if (*s == '\"') + s++; + + // Allocate memory so we can modify the end of the string. + s = strdup(s); + + // Eliminate trailing whitespace + + for (p = s + (strlen(s)-1); p >= s; p--) { + if ((*p == ' ') || (*p == '\t')) { + *p = 0; + } else { + break; + } + } + + // Remove trailing quotes. + if ((p = strrchr(s, '\"')) != NULL) { + *p = 0; + } + + return(s); +} + +#pragma argsused +int main(int argc, char* argv[]) +{ + int dat_argc = 1; + char *dat_name; + char *dst_name; + char *synth_name = NULL; + char filename[512]; + FILE *outfile; + FILE *synthfile; + FILE *dat; + char line[512]; + char *nonwhite; + int synthcount = 0; + int subfiles = 0; + char *product = NULL; + char *method = NULL; + /* + * Read in the descriptions and constraints for synthesis + */ + + if (parse_descr(argv[0])) { + return 1; + } + + /* + * Output MLCad.ini for LSynth + */ + + if (argc == 2 && strcasecmp(argv[1],"-m") == 0) { + char path[256]; + char *l,*p; + int i; + + strcpy(path,argv[0]); + + for (i = 0; i < 2; i++) { + for (p = path; *p; p++) { + if (*p == '\\') { + l = p; + } + } + *l = '\0'; + } + + printf("[LSYNTH]\n"); + printf("%%PATH = \"%s\"\n",path); + product_ini(); + hose_ini(); + band_ini(); + printf("Tangent Statement: INSIDE = SYNTH INSIDE\n"); + printf("Tangent Statement: OUTSIDE = SYNTH OUTSIDE\n"); + printf("Tangent Statement: CROSS = SYNTH CROSS\n"); + printf("Visibility Statement: SHOW = SYNTH SHOW\n"); + printf("Visibility Statement: HIDE = SYNTH HIDE\n"); + return 1; + } + + if (argc == 2 && strcasecmp(argv[1],"-p") == 0) { + printf(argv[0]); + exit(0); + } + + printf("LSynth version %s%s by Kevin Clague, kevin_clague@yahoo.com\n",version,beta); + printf("\n"); // (" and Don Heyse\n"); // Much cleaner. + + if (argc == 2 && strcasecmp(argv[1],"-v") == 0) { + return 1; + } + + /* + * Print out help display + */ + + if (argc == 2 && strcasecmp(argv[1],"-h") == 0 || argc == 1) { + extern void list_hose_types(void); + extern void list_band_types(void); + extern void list_band_constraints(void); + + printf("LSynth is an LDraw compatible flexible part synthesizer\n"); + printf(" usage: lsynthcp [-v] [-h] [-m] [-l] [-p] \n"); + printf(" -v - prints lsynthcp version\n"); + printf(" -h - prints this help message\n"); + printf(" -m - prints out the LSynth portion of the MLcad.ini for using\n"); + printf(" this program\n"); + printf(" -l - format the output as an official ldraw part\n"); + printf(" -p - prints out the full path name of the this executable\n"); + printf("The easiest way to use LSynth is from within MLcad. You need to\n"); + printf("make additions to MLCad.ini. Please see Willy Tscager's tutorial\n"); + printf("page http://www.holly-wood.it/mlcad/lsynth-en.html.\n"); + printf("\n"); + printf("To create a flexible part, you put specifications for the part\n"); + printf("directly into your LDraw file, where the part is needed.\n"); + + list_products(); + list_hose_types(); + list_hose_constraints(); + list_band_types(); + list_band_constraints(); + return 1; + } + + if (strcasecmp(argv[1],"-l") == 0) { + ldraw_part = 1; + dat_argc++; + } + dat_name = argv[dat_argc]; + dst_name = argv[dat_argc+1]; + + if (argc < 3) { + printf("usage: lsynth \n"); + return 1; + } + + dat = fopen(dat_name,"r"); + + if (dat == NULL) { + printf("%s: Failed to open file %s for reading\n",argv[0],dat_name); + return -1; + } + + outfile = fopen(dst_name,"w"); + + if (outfile == NULL) { + printf("%s: Failed to open file %s for writing\n",argv[0],dst_name); + return -1; + } + + /* + * Scan the input file looking for synthesis specifications + */ + + while (L3fgets(line,sizeof(line), dat)) { + + int t1; + + nonwhite = line + strspn(line, " \t"); + strclean(nonwhite); + + if (strncasecmp(nonwhite,"0 SYNTH BEGIN ", + strlen("0 SYNTH BEGIN ")) == 0) { + char *option; + char *group; + char tmp[256]; + int color; + + nonwhite += strlen("0 SYNTH BEGIN "); + + synthfile = outfile; // By default synth data goes to main outfile. + synthcount++; // Count the synth parts. + + // We may be writing synth data to separate subfiles. + if (subfiles) { + // Check if the subfile is named in the SYNTH BEGIN line. + option = strstr(nonwhite, "NAME="); + if (option) { + option += strlen("NAME="); + // Allocate memory for name, strip quotes and trailing spaces. + synth_name = stripquotes(option); + } else { + // Otherwise just number the subfile. Use lsynthN.ldr for stdout? + char *p; + + // Remove the extension from dat_name if not already gone. + if ((p = strrchr(dst_name, '.')) != NULL) + *p = 0; + // Build the new subfilename. + sprintf(filename, "%s%d.ldr", dst_name, synthcount); + synth_name = strdup(filename); + } + + synthfile = fopen(synth_name,"w"); + if (synthfile == NULL) { + printf("%s: Failed to open file %s for writing\n",argv[0],synth_name); + return -1; + } + + fputs(line, synthfile); + } + option = strstr(nonwhite, "LENGTH="); + if (option) { + option += strlen("LENGTH="); + } + option = strstr(nonwhite, "UNITS="); + if (option) { + option += strlen("UNITS="); + } + group = strstr(line, "GROUP="); + if (group) { + char *s; + group += strlen("GROUP="); + s = group; + while (*s && *s != ' ' && *s != '\n') { + s++; + } + *s = '\0'; + } + + /* check to see if it is a known synth command */ + + if (sscanf(nonwhite,"%s %d",tmp,&color) != 2) { + return -1; + } + + product = isproduct(tmp); + if (product) { + fprintf(outfile,"0 LPUB PLI BEGIN SUB %s %d\n",product,color); + method = product_method(product); + } else { + method = tmp; + } + + if ( ! ldraw_part ) { + fputs(line,outfile); + } + + if (ishosetype(method)) { + synth_hose_class(method,color,dat,synthfile,group); + + } else if (isbandtype(method)) { + synth_band_class(method,color,dat,synthfile,group); + + } else { + printf("Unknown synthesis type %s\n",nonwhite); + } + + if (product) { + fprintf(outfile,"0 LPUB PLI END\n"); + printf("Synthesized %s (%s)\n",product,product_nickname(product)); + } else { + printf("Synthesized %s\n",method); + } + + // Close subfile and cleanup + + if (synth_name) { + fclose(synthfile); + free(synth_name); + synth_name = NULL; + } + } else { + float foo,bar; + + if (sscanf(nonwhite,"0 SYNTH HOSE_RES %f %f",&foo,&bar) == 1) { + max_bend = foo; + max_twist = bar; + if ( ! ldraw_part ) { + fputs(line,outfile); + } + } else if (sscanf(nonwhite,"0 SYNTH BAND_RES %f",&foo) == 1) { + band_res = foo; + if ( ! ldraw_part ) { + fputs(line,outfile); + } + } else { + fputs(line,outfile); + } + } + } + fclose(dat); + fclose(outfile); + + printf("lynthcp complete\n"); + return 0; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lsynth/src/lsynthcp.h b/lsynth/src/lsynthcp.h new file mode 100644 index 000000000..dddbd9e16 --- /dev/null +++ b/lsynth/src/lsynthcp.h @@ -0,0 +1,73 @@ +/* + * This file describes the interface to the LDRAW synthesizable parts library. + * Kevin Clague + */ +#ifndef LSYNTH_H +#define LSYNTH_H + +#include +#include +#include +#include + +#include "mathlib.h" +#include "strings.h" + +#define ACCY (1e-6) + +typedef struct { + char type[128]; + PRECISION orient[3][3]; + PRECISION offset[3]; + PRECISION twist; + int attrib; +} part_t; + +extern PRECISION max_bend; +extern PRECISION max_twist; +extern PRECISION band_res; +extern int group_size; +extern int ldraw_part; + +void +output_line( + FILE *output, + int ghost, + char *group, + int color, + PRECISION a, + PRECISION b, + PRECISION c, + PRECISION d, + PRECISION e, + PRECISION f, + PRECISION g, + PRECISION h, + PRECISION i, + PRECISION j, + PRECISION k, + PRECISION l, + char *type); + +void list_products( void ); + +/************************************************************************ + * + * Structures used to define the types of bands and hoses we can synthesize + * and the constraints we can use to describe them. + * + **********************************************************************/ + +#if 1 +// Somehow this broke RCX (STRETCH) cables in 3.1 beta g. +// Maybe what happened is not everything got recompiled... +#define STRETCH -1 +#define FIXED 0 +#define FIXED3 -2 +#else +#define STRETCH 0 +#define FIXED 1 +#define FIXED3 2 +#endif + +#endif diff --git a/lsynth/src/lsynthcp.res b/lsynth/src/lsynthcp.res new file mode 100644 index 0000000000000000000000000000000000000000..e468bf918a523af7a3091b3eec019fa7b1c1693c GIT binary patch literal 876 zcmb_bF;2uV5Zu5uH0X-!NlA%xEfPT&DWU*=!4q&17bzp%{pX4_DNahW&!C{N_=1_S z(;tv z|In#nuj{navMjJhZPaM_QfNJD4x+|BdQI?CM=$p}-Rz^@qE3JH8vXRJG5P7?`N>Z` z^;5UnYGuEjugCL0h&O2V;=F@MIaK@b1)ytaHG_$|4Ry9gjf8ce7r;f$G$71Q;+N=W zkSqKfCkk>9V-oqZ{(-ldmnrWs9vrd4up7|(OL>%gd5|Y?BhQEl(|f_AGs*KsxD~ub GnS25s111yz literal 0 HcmV?d00001 diff --git a/lsynth/src/makefile b/lsynth/src/makefile new file mode 100644 index 000000000..0f770e317 --- /dev/null +++ b/lsynth/src/makefile @@ -0,0 +1,17 @@ +CC=gcc + +CFLAGS=-ggdb + +AR = ar +RANLIB = ranlib + +## This is lame. I don't know how to do both .c and .cpp for the OBJS line +## so I pretend L3*.cpp is L3*.c and make rules for them later. +# +SRCS = lsynthcp.c band.c hose.c curve.c mathlib.c +OBJS = $(SRCS:.c=.o) + +all : lsynthcp + +lsynthcp: $(OBJS) $(LIBS) + $(CC) $(CFLAGS) $(OBJS) -lm -o lsynthcp diff --git a/lsynth/src/mathlib.c b/lsynth/src/mathlib.c new file mode 100644 index 000000000..bd0279ee3 --- /dev/null +++ b/lsynth/src/mathlib.c @@ -0,0 +1,324 @@ + +#include "math.h" +#include "mathlib.h" + +void +vectorcp( + PRECISION dst[3], + PRECISION src[3]) +{ + int i; + + for (i = 0; i < 3; i++) { + dst[i] = src[i]; + } +} + +void +vectoradd3( + PRECISION dst[3], + PRECISION lft[3], + PRECISION rht[3]) +{ + int i; + + for (i = 0; i < 3; i++) { + dst[i] = lft[i] + rht[i]; + } +} + +void +vectoradd( + PRECISION dst[3], + PRECISION src[3]) +{ + int i; + + for (i = 0; i < 3; i++) { + dst[i] += src[i]; + } +} + +void +vectorsub3( + PRECISION dst[3], + PRECISION lft[3], + PRECISION rht[3]) +{ + int i; + + for (i = 0; i < 3; i++) { + dst[i] = lft[i] - rht[i]; + } +} + +void +vectorsub( + PRECISION dst[3], + PRECISION src[3]) +{ + int i; + + for (i = 0; i < 3; i++) { + dst[i] -= src[i]; + } +} + +PRECISION +vectorlen( + PRECISION vect[3]) +{ + PRECISION len = 0; + int i; + + for (i = 0; i < 3; i++) { + len += vect[i]*vect[i]; + } + + return sqrt(len); +} + +void +vectorrot3( + PRECISION t[3], + PRECISION r[3], + PRECISION m[3][3]) +{ +#if 0 + t[0] = r[0]*m[0][0] + r[1]*m[1][0] + r[2]*m[2][0]; + t[1] = r[0]*m[0][1] + r[1]*m[1][1] + r[2]*m[2][1]; + t[2] = r[0]*m[0][2] + r[1]*m[1][2] + r[2]*m[2][2]; +#else + t[0] = r[0]*m[0][0] + r[1]*m[0][1] + r[2]*m[0][2]; + t[1] = r[0]*m[1][0] + r[1]*m[1][1] + r[2]*m[1][2]; + t[2] = r[0]*m[2][0] + r[1]*m[2][1] + r[2]*m[2][2]; +#endif +} + +void +vectorrot( + PRECISION loc[3], + PRECISION m[3][3]) +{ + PRECISION t[3]; + + vectorrot3(t,loc,m); + + loc[0] = t[0]; + loc[1] = t[1]; + loc[2] = t[2]; +} + +void +matrixcp( + PRECISION dst[3][3], + PRECISION src[3][3]) +{ + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + dst[i][j] = src[i][j]; + } + } +} + +int +matrixeq( + PRECISION dst[3][3], + PRECISION src[3][3]) +{ + int i,j,rc = 1; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + rc &= fabs(dst[i][j] - src[i][j]) < 0.0001; + } + } + return rc; +} + +void +matrixneg( + PRECISION dst[3][3], + PRECISION src[3][3]) +{ + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + dst[i][j] = -src[i][j]; + } + } +} + +void +matrixadd3( + PRECISION dst[3][3], + PRECISION lft[3][3], + PRECISION rht[3][3]) +{ + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + dst[i][j] = lft[i][j] + rht[i][j]; + } + } +} + +void +matrixadd( + PRECISION dst[3][3], + PRECISION src[3][3]) +{ + int i,j; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + dst[i][j] += src[i][j]; + } + } +} + +void +matrixmult3( + PRECISION res[3][3], + PRECISION lft[3][3], + PRECISION rht[3][3]) +{ + int i,j,k; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + res[i][j] = 0.0; + } + } + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + for (k = 0; k < 3; k++) { + res[i][j] += lft[i][k] * rht[k][j]; + } + } + } +} + +void +matrixmult( + PRECISION res[3][3], + PRECISION src[3][3]) +{ + PRECISION t[3][3]; + + matrixcp(t,res); + matrixmult3(res,t,src); +} + +void +matrixinv2( + PRECISION inv[3][3], + PRECISION src[3][3]) +{ + int i; + PRECISION tmp[3][3]; + + for (i = 0; i < 3; i++) { + int j; + for (j = 0; j < 3; j++) { + inv[i][j] = 0; + } + inv[i][i] = 1; + } + + matrixcp(tmp,src); + + for (i = 0; i < 3; i++) { + int j; + PRECISION l; + + l = tmp[i][i]; + + if (l == 0) { + break; + } else { + int a; + for (j = 0; j <= 3-1; j++) { + tmp[i][j] /= l; + inv[i][j] /= l; + } + for (a = 0; a < 3; a++) { + if ((a-i) != 0) { + PRECISION b; + b = tmp[a][i]; + for (j = 0; j < 3; j++) { + tmp[a][j] -= b*tmp[i][j]; + inv[a][j] -= b*inv[i][j]; + } + } + } + } + } +} + +void +matrixinv( + PRECISION a[3][3], + PRECISION src[3][3]) +{ + int i,j,k; + int p[3]; + PRECISION h,q,s,sup,pivot; + + matrixcp(a,src); + + for (k = 0; k < 3; k++) { + sup = 0.0; + p[k] = 0; + for (i = k; i < 3; i++) { + s = 0.0; + for (j = k; j < 3; j++) { + s += fabs(a[i][j]); + } + q = fabs(a[i][k])/s; + if (sup < q) { + sup = q; + p[k] = i; + } + } + if (sup == 0.0) { + return; + } + if (p[k] != k) { + for (j = 0; j < 3; j++) { + h = a[k][j]; + a[k][j] = a[p[k]][j]; + a[p[k]][j] = h; + } + } + pivot = a[k][k]; + for (j = 0; j < 3; j++) { + if (j != k) { + a[k][j] = -a[k][j] / pivot; + for (i = 0; i < 3; i++) { + if (i != k) { + a[i][j] += a[i][k] * a[k][j]; + } + } + } + } + for (i = 0; i < 3; i++) { + a[i][k] /= pivot; + } + a[k][k] = 1/pivot; + } + for (k = 3 - 1; k >= 0; k--) { + if (p[k] != k) { + for (i = 0; i < 3; i++) { + h = a[i][k]; + a[i][k] = a[i][p[k]]; + a[i][p[k]] = h; + } + } + } +} diff --git a/lsynth/src/mathlib.h b/lsynth/src/mathlib.h new file mode 100644 index 000000000..77534f365 --- /dev/null +++ b/lsynth/src/mathlib.h @@ -0,0 +1,25 @@ +#ifndef MATHLIB +#define MATHLIB + +#define PRECISION float + +void vectorcp(PRECISION dst[3],PRECISION src[3]); +void vectoradd3(PRECISION dst[3],PRECISION lft[3], PRECISION rht[3]); +void vectoradd(PRECISION lft[3],PRECISION rht[3]); +void vectorsub3(PRECISION dst[3],PRECISION lft[3], PRECISION rht[3]); +void vectorsub(PRECISION lft[3],PRECISION rht[3]); +PRECISION vectorlen(PRECISION vect[3]); +void vectorrot3(PRECISION res[3],PRECISION src[3],PRECISION rot[3][3]); +void vectorrot(PRECISION loc[3],PRECISION m[3][3]); + +void matrixcp(PRECISION dst[3][3],PRECISION src[3][3]); +void matrixadd(PRECISION dst[3][3],PRECISION src[3][3]); +void matrixadd3(PRECISION res[3][3],PRECISION lft[3][3],PRECISION rht[3][3]); +void matrixmult3(PRECISION res[3][3],PRECISION lft[3][3],PRECISION rht[3][3]); +void matrixmult(PRECISION res[3][3],PRECISION src[3][3]); +void matrixinv(PRECISION inv[3][3],PRECISION src[3][3]); +void matrixneg(PRECISION neg[3][3],PRECISION src[3][3]); +int matrixeq(PRECISION dst[3][3],PRECISION src[3][3]); + +#endif + diff --git a/lsynth/src/orient.c b/lsynth/src/orient.c new file mode 100644 index 000000000..39d1f6540 --- /dev/null +++ b/lsynth/src/orient.c @@ -0,0 +1,94 @@ +void orient( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION start_up[3],end_up[3],up[3]; + PRECISION total_length = hose_length(n_segments,segments); + PRECISION cur_length; + int i; + + start_up[0] = 1; // Why is X the up vector? + start_up[1] = 0; // The base constraint points up -Y, + start_up[2] = 0; // And the up fin lies on -Z. + rotate_point(start_up,start->orient); + + end_up[0] = 1; + end_up[1] = 0; + end_up[2] = 0; + rotate_point(end_up,end->orient); + + /* Up vector controls the twist + * + * Interpolate the up vector based on start up vector, and + * end up vector, and how far we are down the hose's total length + */ + + for (i = 0; i < n_segments-1; i++) { + PRECISION r; + PRECISION front[3]; + PRECISION t[3]; + + cur_length = hose_length(i,segments); + + cur_length /= total_length; + + //Interpolate up vector from start to end based on length traversed. + up[0] = start_up[0]*(1-cur_length) + end_up[0]*cur_length; + up[1] = start_up[1]*(1-cur_length) + end_up[1]*cur_length; + up[2] = start_up[2]*(1-cur_length) + end_up[2]*cur_length; + + r = sqrt(up[0]*up[0] + up[1]*up[1] + up[2]*up[2]); + up[0] /= r; + up[1] /= r; + up[2] /= r; + + // Get the current forward direction vector. + front[0] = segments[i+1].offset[0] - segments[i].offset[0]; + front[1] = segments[i+1].offset[1] - segments[i].offset[1]; + front[2] = segments[i+1].offset[2] - segments[i].offset[2]; + + r = sqrt(front[0]*front[0] + front[1]*front[1] + front[2]*front[2]); + + front[0] /= r; + front[1] /= r; + front[2] /= r; + + //Use cross products to create 2 Euler rotation axes? gimble lock? + mult_point(t,front,up); // side + mult_point(up,t,front); // side * front + + //Take cross product (Up x Front) and make the rotation matrix from it. + make_rotation_pp(segments[i].orient,up,front); + } +} + + // ---------------------------------------------------------------- + // We have a problem. + + // Interpolated up and forward direction vector may not be perpendicular. + + // Using quaternions + // ---------------- + // Using forward vectors S (start) and V (front): + // Calculate turn axis from cross product. (S x V) / |(S x V)| + // Then get the total turn angle from dot product. arccos((S . V)/ |S|*|V|) + // Make quaternion Q from axis and angle. + // Make rotation R matrix from quaternion. + // Multiply start matrix by new matrix to get orient for segment. + + // Now for the spin: + // ---------------- + // Call whatever spin is built into S a start spin of 0. + // On the last segment above V = E. Keep the last rotation matrix R. + // Using up vectors s (start) and e (end): + // Rotate s by R to get u. + // Use dot product of u and e to get delta spin angle from s to e. + // Interpolate the delta spin along the length of the hose. + // We can apply the interpolated spin as a rotation around Y axis before + // applying the orient matrix of each segment. + // (Just add it to the per segment spin we already have). + // ---------------------------------------------------------------- + + diff --git a/lsynth/src/quat.c b/lsynth/src/quat.c new file mode 100644 index 000000000..8cc7fea9c --- /dev/null +++ b/lsynth/src/quat.c @@ -0,0 +1,603 @@ +/* + * This is the LDRAW parts synthesis library. + * By Kevin Clague + */ + +#include "lsynthcp.h" +#include "hose.h" +#include "curve.h" +#include "mathlib.h" + +static void +mult_point(PRECISION r[3], PRECISION lhs[3], PRECISION rhs[3]) +{ + PRECISION tt; + + r[0] = lhs[1]*rhs[2] - lhs[2]*rhs[1]; + r[1] = lhs[2]*rhs[0] - lhs[0]*rhs[2]; + r[2] = lhs[0]*rhs[1] - lhs[1]*rhs[0]; + + tt = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); + + r[0] /= tt; + r[1] /= tt; + r[2] /= tt; +} + +static void +make_rotation_pp( + PRECISION r[3][3], + PRECISION up[3], + PRECISION front[3]) +{ + PRECISION side[3]; + + mult_point(side,front,up); + + r[0][0] = up[0]; + r[1][0] = up[1]; + r[2][0] = up[2]; + r[0][1] = front[0]; + r[1][1] = front[1]; + r[2][1] = front[2]; + r[0][2] = side[0]; + r[1][2] = side[1]; + r[2][2] = side[2]; +} + +static void +rotate_point( + PRECISION r[3], + PRECISION m[3][3]) +{ + PRECISION t[3]; + + t[0] = r[0]*m[0][0] + r[1]*m[0][1] + r[2]*m[0][2]; + t[1] = r[0]*m[1][0] + r[1]*m[1][1] + r[2]*m[1][2]; + t[2] = r[0]*m[2][0] + r[1]*m[2][1] + r[2]*m[2][2]; + + r[0] = t[0]; + r[1] = t[1]; + r[2] = t[2]; +} + +void +orient2( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION up[3]; + int i; + + up[0] = 1; + up[1] = 0; + up[2] = 0; + + rotate_point(up,start->orient); + printf("%5.2f %5.2f %5.2f\n",up[0],up[1],up[2]); + + for (i = 0; i < n_segments-1; i++) { + PRECISION r; + PRECISION front[3]; + PRECISION t[3]; + + front[0] = segments[i+1].offset[0] - segments[i].offset[0]; + front[1] = segments[i+1].offset[1] - segments[i].offset[1]; + front[2] = segments[i+1].offset[2] - segments[i].offset[2]; + + r = sqrt(front[0]*front[0] + front[1]*front[1] + front[2]*front[2]); + + front[0] /= r; + front[1] /= r; + front[2] /= r; + + mult_point(t,front,up); // side + mult_point(up,t,front); // side * front + + make_rotation_pp(segments[i].orient,up,front); + } +} + +PRECISION hose_length( + int n_segments, + part_t *segments) +{ + PRECISION length = 0; + int i; + + for (i = 0; i < n_segments-1; i++) { + PRECISION l[3]; + + vectorsub3(l,segments[i+1].offset,segments[i].offset); + + length += vectorlen(l); + } + return length; +} + +void +orient( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION start_up[3],end_up[3],up[3]; + PRECISION total_length = hose_length(n_segments,segments); + PRECISION cur_length; + int i; + + start_up[0] = 1; + start_up[1] = 0; + start_up[2] = 0; + rotate_point(start_up,start->orient); + + end_up[0] = 1; + end_up[1] = 0; + end_up[2] = 0; + rotate_point(end_up,end->orient); + + /* Up vector controls the twist + * + * Interpolate the up vector based on start up vector, and + * end up vector, and how far we are down the hose's total + * length + */ + + for (i = 0; i < n_segments-1; i++) { + PRECISION r; + PRECISION front[3]; + PRECISION t[3]; + + cur_length = hose_length(i,segments); + + cur_length /= total_length; + + up[0] = start_up[0]*(1-cur_length) + end_up[0]*cur_length; + up[1] = start_up[1]*(1-cur_length) + end_up[1]*cur_length; + up[2] = start_up[2]*(1-cur_length) + end_up[2]*cur_length; + + r = sqrt(up[0]*up[0] + up[1]*up[1] + up[2]*up[2]); + up[0] /= r; + up[1] /= r; + up[2] /= r; + + front[0] = segments[i+1].offset[0] - segments[i].offset[0]; + front[1] = segments[i+1].offset[1] - segments[i].offset[1]; + front[2] = segments[i+1].offset[2] - segments[i].offset[2]; + + r = sqrt(front[0]*front[0] + front[1]*front[1] + front[2]*front[2]); + + front[0] /= r; + front[1] /= r; + front[2] /= r; + + mult_point(t,front,up); // side + mult_point(up,t,front); // side * front + + make_rotation_pp(segments[i].orient,up,front); + } +} + +int +synth_curve( + part_t *start, + part_t *end, + part_t *segments, + int n_segments, + PRECISION attrib, + FILE *output) +{ + PRECISION vector[3]; + PRECISION start_speed_v[3]; + PRECISION stop_speed_v[3]; + PRECISION time,time2,i_time,i_time_sum,i_time_sum2,n_time; + PRECISION x,x2,y,y2,z,z2; + PRECISION ptp,ratio; + PRECISION ptp_sum; + int i,j,n; + + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x += start->orient[j][i] * vector[i]; + } + start_speed_v[j] = x; + } + + vector[0] = 0; + vector[1] = attrib; + vector[2] = 0; + + for (j = 0; j < 3; j++) + { + x = 0.0; + for (i = 0; i < 3; i++) { + x -= end->orient[j][i] * vector[i]; + } + stop_speed_v[j] = x; + } + + // stop_speed_v[2] = -stop_speed_v[2]; + + for (i = 0; i < n_segments; i++) { + time = (PRECISION) i/ (PRECISION) n_segments; + + segments[i].offset[0] = + (1 - time) * (1 - time) * (1 - time) * start->offset[0] + + (1 - time) * (1 - time) * 3 * time * (start->offset[0] - start_speed_v[0]) + + (1 - time) * 3 * time * time * (end->offset[0] - stop_speed_v[0]) + + time * time * time * end->offset[0]; + + segments[i].offset[1] = + (1 - time) * (1 - time) * (1 - time) * start->offset[1] + + (1 - time) * (1 - time) * 3 * time * (start->offset[1] - start_speed_v[1]) + + (1 - time) * 3 * time * time * (end->offset[1] - stop_speed_v[1]) + + time * time * time * end->offset[1]; +/* +=(1-$A8)^3*D$4 + (1-$A8)^2*3*$A8*(D$4-D$3) + (1-$A8)*3*$A8^2*(D$5-D$6) + $A8^3*D$5 + */ + segments[i].offset[2] = + (1 - time) * (1 - time) * (1 - time) * start->offset[2] + + (1 - time) * (1 - time) * 3 * time * (start->offset[2] - start_speed_v[2]) + + (1 - time) * 3 * time * time * (end->offset[2] - stop_speed_v[2]) + + time * time * time * end->offset[2]; + } + + ptp_sum = 0; + + for (i = 0; i < n_segments - 1; i++) { + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + ptp_sum += ptp; + } + + /* G8 */ + + i_time_sum = 0; + for (i = 0; i < n_segments - 1; i++) { + time = (PRECISION) (i+1)/ (PRECISION) n_segments; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x*x + y*y + z*z); + + ratio = ptp*n_segments/ptp_sum; + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); + i_time_sum += i_time; + } + + /* H, I, J */ + n_time = 0; + i_time_sum2 = 0; + for (i = 0; i < n_segments - 1; i++) { + PRECISION foo; + + x = segments[i + 1].offset[0] - segments[i].offset[0]; + y = segments[i + 1].offset[1] - segments[i].offset[1]; + z = segments[i + 1].offset[2] - segments[i].offset[2]; + + ptp = sqrt(x * x + y * y + z * z); /* E */ + ratio = ptp*n_segments/ptp_sum; /* F */ + if (ratio == 0) { + ratio = 1e-20; + } + i_time = 1.0/(n_segments*ratio); /* G */ + i_time_sum2 += i_time; + + foo = 1.0/n_segments; + foo /= ratio; + foo /= i_time_sum; + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + n_time += foo; /* H */ + } + + segments[i].offset[0] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[0] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[0] - start_speed_v[0]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[0] - stop_speed_v[0]) + + n_time * n_time * n_time * end->offset[0]; + + segments[i].offset[1] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[1] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[1] - start_speed_v[1]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[1] - stop_speed_v[1]) + + n_time * n_time * n_time * end->offset[1]; + + segments[i].offset[2] = + (1 - n_time) * (1 - n_time) * (1 - n_time) * start->offset[2] + + (1 - n_time) * (1 - n_time) * 3 * n_time * (start->offset[2] - start_speed_v[2]) + + (1 - n_time) * 3 * n_time * n_time * (end->offset[2] - stop_speed_v[2]) + + n_time * n_time * n_time * end->offset[2]; + + // orient(n_segments, segments); + + return 0; /* it all worked */ +} + + + + + +/***************************************************************/ +// Quaternion q[4] is mostly a direction vector with a spin(roll) angle. +// ie. quaternion(x,y,z,spin) +// +// Gotta compare quat FAQ conversions to ldglite conversions to +// see what to do with the weird ldraw coordinate system. I think I +// may have to reverse -y (and maybe z) to get right? handed system +// before using quaternions. Make sure I copied all conversions from +// same place to avoid mixed systems. + +/***************************************************************/ + +/***************************************************************/ +void normalizequat(PRECISION q[4]) +{ + PRECISION L; + + L = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + + q[0] /= L; + q[1] /= L; + q[2] /= L; + q[3] /= L; +} + +/***************************************************************/ +void normalize(PRECISION v[3]) +{ + PRECISION L; + + L = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + + v[0] /= L; + v[1] /= L; + v[2] /= L; +} + +/***************************************************************/ +// Convert axis v[3] and angle a to quaternion q[4]. +/***************************************************************/ +void axisangle2quat( + PRECISION v[3], + PRECISION a, + PRECISION q[4]) +{ + PRECISION sina,cosa; + + a = a / 2.0; + sina = sin(a); + cosa = cos(a); + q[0] = v[0] * sina; + q[1] = v[1] * sina; + q[2] = v[2] * sina; + q[3] = cosa; +} + +/***************************************************************/ +// Convert rotation matrix m[3][3] to quaternion q[4]. +/***************************************************************/ +void matrix2quat( + PRECISION m[3][3], + PRECISION q[4]) +{ + PRECISION T; + PRECISION S; + + // Calculate the trace of the matrix T from the equation: + T = 1 + m[0][0] + m[1][1] + m[2][2]; + if ( T > 0.00000001 ) + { + S = sqrt(T) * 2; + q[0] = ( m[2][1] - m[1][2] ) / S; + q[1] = ( m[0][2] - m[2][0] ) / S; + q[2] = ( m[1][0] - m[0][1] ) / S; + q[3] = 0.25 * S; + } + // If the trace of the matrix is equal to zero then identify + // which major diagonal element has the greatest value. + // Depending on this, calculate the following: + else if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) { // Column 0: + S = sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2] ) * 2; + q[0] = 0.25 * S; + q[1] = ( m[1][0] + m[0][1] ) / S; + q[2] = ( m[0][2] + m[2][0] ) / S; + q[3] = ( m[2][1] - m[1][2] ) / S; + } else if ( m[1][1] > m[2][2] ) { // Column 1: + S = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2; + q[0] = ( m[1][0] + m[0][1] ) / S; + q[1] = 0.25 * S; + q[2] = ( m[2][1] + m[1][2] ) / S; + q[3] = ( m[0][2] - m[2][0] ) / S; + } else { // Column 2: + S = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2; + q[0] = ( m[0][2] + m[2][0] ) / S; + q[1] = ( m[2][1] + m[1][2] ) / S; + q[2] = 0.25 * S; + q[3] = ( m[1][0] - m[0][1] ) / S; + } +} + +/***************************************************************/ +// Convert quaternion q[4] to rotation matrix m[3][3]. +/***************************************************************/ +void quat2matrix( + PRECISION q[4], + PRECISION m[3][3]) +{ + PRECISION a,b,c,s; + + normalizequat( q ); + + a = q[0]; + b = q[1]; + c = q[2]; + s = q[3]; + + m[0][0] = 1 - 2*b*b-2*c*c; + m[0][1] = 2*a*b - 2*s*c; + m[0][2] = 2*a*c + 2*s*b; + m[1][0] = 2*a*b+2*s*c; + m[1][1] = 1 - 2*a*a - 2*c*c; + m[1][2] = 2*b*c - 2*s*a; + m[2][0] = 2*a*c - 2*s*b; + m[2][1] = 2*b*c + 2*s*a; + m[2][2] = 1 - 2*a*a - 2*b*b; +} + +/***************************************************************/ +// Convert quaternion q[4] to a rotation axis v[3] and angle a. +/***************************************************************/ + +PRECISION quat2axisangle( + PRECISION q[4], + PRECISION v[3], + PRECISION *a) +{ + PRECISION cos_angle, sin_angle; + + normalizequat( q ); + + cos_angle = q[3]; + *a = acos( cos_angle ) * 2; + sin_angle = sqrt( 1.0 - cos_angle * cos_angle ); + + if ( fabs( sin_angle ) < 0.0005 ) + sin_angle = 1; + + v[0] = q[0] / sin_angle; + v[1] = q[1] / sin_angle; + v[2] = q[2] / sin_angle; + + return *a; +} + +/***************************************************************/ +// Convert start, end constraints and n points to oriented segments +// using quaternion. +/***************************************************************/ +void +orientq( + part_t *start, + part_t *end, + int n_segments, + part_t *segments) +{ + PRECISION start_up,end_up,up; // Up is now a twist angle, not a vector. + PRECISION start_q[4],end_q[4],q[4]; + PRECISION v[3]; + PRECISION total_length = hose_length(n_segments,segments); + PRECISION cur_length; + int i; + + // Get the start and end twist angles (up vectors) + matrix2quat(start->orient, start_q); + matrix2quat(end->orient, end_q); + +#define DEBUG_QUAT_MATH 1 +#ifdef DEBUG_QUAT_MATH + { + PRECISION pi = 2*atan2(1,0); + PRECISION degrees = 180.0 / pi; + + quat2axisangle(start_q, v, &start_up); + quat2axisangle(end_q, v, &end_up); + printf("Start_q = (%.2f, %.2f, %.2f, %.2f) up = %.2f degrees\n", + start_q[0],start_q[1],start_q[2],start_q[3], start_up*degrees); + } +#else + start_up = acos(start_q[3]) * 2.0; + end_up = acos(end_q[3]) * 2.0; +#endif + + /* Up angle controls the twist + * + * Interpolate the up angle based on start up angle, end up angle, + * and how far we are down the hose's total length. + */ + + for (i = 1; i < n_segments; i++) { + PRECISION v0[3]; + + cur_length = hose_length(i,segments); + cur_length /= total_length; + up = start_up*(1-cur_length) + end_up*cur_length; + + // Get the axis before segment i. + v0[0] = segments[i].offset[0] - segments[i-1].offset[0]; + v0[1] = segments[i].offset[1] - segments[i-1].offset[1]; + v0[2] = segments[i].offset[2] - segments[i-1].offset[2]; + normalize(v0); + + // Get the axis after segment i. + v[0] = segments[i+1].offset[0] - segments[i].offset[0]; + v[1] = segments[i+1].offset[1] - segments[i].offset[1]; + v[2] = segments[i+1].offset[2] - segments[i].offset[2]; + normalize(v); + + // Tangent at segment i is the average of the before and after axis. + v[0] += v0[0]; + v[1] += v0[1]; + v[2] += v0[2]; + normalize(v); + + // Convert to a quaternion, and convert that to a rotation matrix. + axisangle2quat(v, up, q); + normalizequat( q ); + quat2matrix(q, segments[i].orient); + +#ifdef DEBUG_QUAT_MATH + { + PRECISION pi = 2*atan2(1,0); + PRECISION degrees = 180.0 / pi; + + printf(" V[%d] = (%.2fx, %.2fy, %.2fz, %.2f up) => Q(%.2f, %.2f, %.2f, %.2f)\n", + i,v[0],v[1],v[2], up*degrees, q[0],q[1],q[2],q[3]); + } +#endif + } +#ifdef DEBUG_QUAT_MATH + { + PRECISION pi = 2*atan2(1,0); + PRECISION degrees = 180.0 / pi; + + printf("End_q = (%.2f, %.2f, %.2f, %.2f) up = %.2f degrees\n", + end_q[0],end_q[1],end_q[1],end_q[1], end_up*degrees); + } +#endif +} + diff --git a/lsynth/src/strings.c b/lsynth/src/strings.c new file mode 100644 index 000000000..25580070a --- /dev/null +++ b/lsynth/src/strings.c @@ -0,0 +1,43 @@ +#include + +int strncasecmp( + const char *s1, + const char *s2, + int n) +{ + unsigned c1, c2; + + if (n == 0) { + return 0; + } + + do { + c1 = tolower(*s1++); + c2 = tolower(*s2++); + + if (c1 != c2) + return c1 - c2; + if (c1 == 0) + break; + } while (--n != 0); + return 0; +} + +int strcasecmp( + const char *s1, + const char *s2) +{ + unsigned c1, c2; + + do { + c1 = tolower(*s1++); + c2 = tolower(*s2++); + + if (c1 != c2) + return c1 - c2; + if (c1 == 0) + break; + } while(1); + + return 0; +} diff --git a/lsynth/src/strings.h b/lsynth/src/strings.h new file mode 100644 index 000000000..de003721f --- /dev/null +++ b/lsynth/src/strings.h @@ -0,0 +1,21 @@ +#ifndef casecmp_h +#define casecmp_h + +#ifdef WIN32 +#ifdef __MINGW_H + // These are in string.h in mingw version of gcc. + // Others may need to include but it usually conflicts with + // Maybe we should move the #include from lsynthcp.h to here. +#else +int strncasecmp( + const char *s1, + const char *s2, + int n); + +int strcasecmp( + const char *s1, + const char *s2); +#endif +#endif + +#endif