Username404-59
/
ldraw
mirror of https://github.com/pybricks/ldraw.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

lsynth: Import 3.1 source.

This commit is contained in:
Laurens Valk 2020-09-20 16:38:05 +02:00
parent e51928603d
commit bb14499ee3
No known key found for this signature in database
GPG Key ID: C5FCCBB5E9FB117A
17 changed files with 7233 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1415
lsynth/src/band.c Normal file
View File

File diff suppressed because it is too large Load Diff

73
lsynth/src/band.h Normal file
View File

@ -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

1038
lsynth/src/curve.c Normal file
View File

File diff suppressed because it is too large Load Diff

44
lsynth/src/curve.h Normal file
View File

@ -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

440
lsynth/src/curvy.c Normal file
View File

@ -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 */
}

889
lsynth/src/hose.c Normal file
View File

@ -0,0 +1,889 @@
/*
* This is the LDRAW parts synthesis library.
* By Kevin Clague and Don Heyse
*/
#include <math.h>
#include "lsynthcp.h"
#include "hose.h"
#include "curve.h"
#include "mathlib.h"
#define PI 2*atan2(1,0)
/*
* 0 SYNTH BEGIN DEFINE HOSE <fill> PNEUMATIC_HOSE "descr" <diameter> <stiffness> <twist>
* 1 <length> a b c d e f g h i j k l <part>
* 1 <length> a b c d e f g h i j k l <part>
* 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 <length> a b c d e f g h i j k l <part>
* 1 <length> a b c d e f g h i j k l <part>
* 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;
}

50
lsynth/src/hose.h Normal file
View File

@ -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

2084
lsynth/src/lsynthcp.c Normal file
View File

File diff suppressed because it is too large Load Diff

73
lsynth/src/lsynthcp.h Normal file
View File

@ -0,0 +1,73 @@
/*
* This file describes the interface to the LDRAW synthesizable parts library.
* Kevin Clague
*/
#ifndef LSYNTH_H
#define LSYNTH_H
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#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

BIN
lsynth/src/lsynthcp.res Normal file
View File

Binary file not shown.

17
lsynth/src/makefile Normal file
View File

@ -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

324
lsynth/src/mathlib.c Normal file
View File

@ -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;
}
}
}
}

25
lsynth/src/mathlib.h Normal file
View File

@ -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

94
lsynth/src/orient.c Normal file
View File

@ -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).
// ----------------------------------------------------------------

603
lsynth/src/quat.c Normal file
View File

@ -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
}

43
lsynth/src/strings.c Normal file
View File

@ -0,0 +1,43 @@
#include <ctype.h>
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;
}

21
lsynth/src/strings.h Normal file
View File

@ -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 <strings.h> but it usually conflicts with <string.h>
// Maybe we should move the #include <string.h> 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