<![CDATA[
/********************************************************
Algorithm FBRBG3-1
Purpose: Design a rigid-body guidance four-bar linkage
for 3 prescribed positions of the coupler
Input parameters:
xp,yp: Coordinates of the coupler point
theta: Coupler rotations (deg)
X0,Y0: Center point coordinates
Output parameters:
X1,Y1: Circle point coordinates
Written by Ettore Pennestri - Universita Roma Tor Vergata
e-mail: pennestri@mec.uniroma2.it (March 3rd 2003)
********************************************************/
#include <math.h>
#include <array.h>
#include <numeric.h>
#include <stdio.h>
#include <fourbar.h>
void fbrbg3(array double xp[3],array double yp[3],array double thetai[2],
double X0, double Y0,double &X1,double &Y1) {
array double r[2],s[2],a[2][2],b[2],x[2],theta[2];
int i;
for(i=0;i<=1;i++){
theta[i]=thetai[i]*M_PI/180.;
r[i]=xp[i+1]-xp[0]*cos(theta[i])+yp[0]*sin(theta[i]);
s[i]=yp[i+1]-xp[0]*sin(theta[i])-yp[0]*cos(theta[i]);
}
for(i=0;i<=1;i++){
a[i][0]=r[i]*cos(theta[i])+s[i]*sin(theta[i])-X0*cos(theta[i])-Y0*sin(theta[i])+X0;
a[i][1]=s[i]*cos(theta[i])-r[i]*sin(theta[i])+X0*sin(theta[i])-Y0*cos(theta[i])+Y0;
b[i]=r[i]*X0+s[i]*Y0-0.5*(r[i]*r[i]+s[i]*s[i]);
}
//printf("a \n %f \n ",a);
//printf("b \n %f \n ",b);
linsolve(x,a,b);
X1=x[0];Y1=x[1];
}
/* Compute four-bar link lengths and initial angular positions */
void extract_fourbar(array double a0x[2], array double a0y[2], array double a1x[2],
array double a1y[2], double &r1, double &r2, double &r3, double &r4,
double &theta1, double &theta2, double &theta3, double &theta4){
array double r1v[2],r2v[2],r3v[2],r4v[2];
r1v[0]=a0x[1]-a0x[0];
r1v[1]=a0y[1]-a0y[0];
r1=norm(r1v,"2");
theta1=atan2(r1v[1],r1v[0]);
r2v[0]=a1x[0]-a0x[0];
r2v[1]=a1y[0]-a0y[0];
r2=norm(r2v,"2");
theta2=atan2(r2v[1],r2v[0]);
r3v[0]=a1x[1]-a1x[0];
r3v[1]=a1y[1]-a1y[0];
r3=norm(r3v,"2");
theta3=atan2(r3v[1],r3v[0]);
r4v[0]=a1x[1]-a0x[1];
r4v[1]=a1y[1]-a0y[1];
r4=norm(r4v,"2");
theta4=atan2(r4v[1],r4v[0]);
}
int main(){
array double xp[3], yp[3],thetai[2];
array double a0x[2],a0y[2],a1x[2],a1y[2];
double r1,r2,r3,r4,theta1,theta2,theta3,theta4;
int n1 = 2, n2 = 4, ibranch=0;
double theta3a,theta4a,theta2a,theta2b,theta3b,theta4b;
double xlow=0.0001;
double complex z;
FILE *file2;
file2 = fopen("fbrbg3.txt","w");
FILE *file3;
file3 = fopen("fourbar_animation.qnm","w");
/* Assign coupler positions */
xp[0]=121;yp[0]=52;
xp[1]=101;yp[1]=112;
xp[2]=21;yp[2]=132;
thetai[0]=40;thetai[1]=90;
/* Assign circle point coordinates */
a0x[0]=51.4;a0y[0]=51.6;
a0x[1]=38.0;a0y[1]=58.5;
int i;
/* Compute circle point coordinates */
for(i=0;i<=1;i++){
fbrbg3(xp,yp,thetai,a0x[i],a0y[i],a1x[i],a1y[i]);
}
extract_fourbar(a0x,a0y,a1x,a1y,r1,r2,r3,r4,
theta1,theta2,theta3,theta4);
// Print results
fprintf(file2,"\n Synthesis of rigid body guidance four-bar linkage \n");
fprintf(file2,"\n 3 finite positions \n");
fprintf(file2,"\n \n");
fprintf(file2,"DATA: \n");
fprintf(file2,"\n Coupler point coordinates: \n");
int j;
for(i=0;i<=2;i++){
j=i+1;
fprintf(file2,"x[%i]= %f y[%i]= %f \n",j,xp[i],j,yp[i]);}
fprintf(file2,"\n Coupler rotation (deg): \n ");
fprintf(file2,"theta12=%f theta13=%f \n",thetai[0],thetai[1]);
fprintf(file2,"Center point #1: A0x=%f A0y=%f \n",a0x[0],a0y[0]);
fprintf(file2,"Center point #2: B0x=%f B0y=%f \n",a0x[1],a0y[1]);
fprintf(file2,"RESULTS \n");
fprintf(file2,"Circle point #1: A1x=%f A1y=%f \n",a1x[0],a1y[0]);
fprintf(file2,"Circle point #2: B1x=%f B1y=%f \n",a1x[1],a1y[1]);
fprintf(file2,"\n Four-bar dimensions and angles: \n");
fprintf(file2,"r1=%f theta1=%f deg \n",r1,M_RAD2DEG(theta1));
fprintf(file2,"r2=%f theta2=%f deg \n",r2,M_RAD2DEG(theta2));
fprintf(file2,"r3=%f theta3=%f deg \n",r3,M_RAD2DEG(theta3));
fprintf(file2,"r4=%f theta4=%f deg \n",r4,M_RAD2DEG(theta4));
// Compute branch number
z = polar(r1,theta1) - polar(r3,theta3);
complexsolve(n1,n2,r2,-r4,z,theta2a, theta4a, theta2b, theta4b);
// fprintf(file2,"Solution n.1: theta2a=%f theta4a=%f deg \n",M_RAD2DEG(theta2a),M_RAD2DEG(theta4a));
// fprintf(file2,"Solution n.2: theta2b=%f theta4b=%f deg \n",M_RAD2DEG(theta2b),M_RAD2DEG(theta4b));
if(abs(theta2-theta2a)<xlow){ibranch=1;}
if(abs(theta2-theta2b)<xlow){ibranch=2;}
fprintf(file2,"ibranch=%i ",ibranch);
double r0=sqrt(a0x[0]*a0x[0]+a0y[0]*a0y[0]);
double theta0=atan2(a0y[0],a0x[0]);
double rp=sqrt(pow(xp[0]-a1x[0],2)+pow(yp[0]-a1y[0],2));
double beta=atan2(yp[0]-a1y[0],xp[0]-a1x[0])-theta3;
// Animation
complex R[0:3]; // link vectors
complex P; // coupler vector
double t3;
R[0] = polar(r0,theta0);
R[1] = R[0]+polar(r1,theta1);
/* The first line of the animation file must start with #qanimate */
fprintf(file3,"#qanimate animation data\n");
/* The title displayed on the animation */
fprintf(file3,"title Synthesis \n");
fprintf(file3,"fixture\n");
/* The primitives following fixture */
fprintf(file3,"groundpin %g %g %g %g\n", real(R[0]), imag(R[0]),real(R[1]), imag(R[1]));
/* Draw rectangles representing the positions of the body */
double rectx=10;double recty=30;
fprintf(file3,"rectangle %g %g %g %g angle 0 fill red \n", xp[0], yp[0],rectx, recty);
fprintf(file3,"rectangle %g %g %g %g angle %g fill red \n", xp[1], yp[1],rectx, recty,thetai[0]);
fprintf(file3,"rectangle %g %g %g %g angle %g fill red \n", xp[2], yp[2],rectx, recty,thetai[1]);
fprintf(file3,"\n");
/* For crank-rocker, crank-crank, use animate restart.
For rocker-rocker, rocker-crank, use animate reverse */
fprintf(file3,"animate reverse\n");
double theta3max=theta3+M_DEG2RAD(thetai[1]);
double theta3min=theta3;
double dtheta3=(theta3max-theta3min)/100;
for(theta3=theta3min;theta3<=theta3max;theta3=theta3+dtheta3){
z = polar(r1,theta1) - polar(r3,theta3);
complexsolve(n1,n2,r2,-r4,z,theta2a, theta4a, theta2b, theta4b);
if(ibranch==1){
R[2] = R[0]+polar(r2,theta2a);
R[3] = R[2] + polar(r3,theta3);
P = R[2] + polar(rp,theta3+beta);
}
if(ibranch==2){
R[2] = R[0]+polar(r2,theta2b);
R[3] = R[2] + polar(r3,theta3);
P = R[2] + polar(rp,theta3+beta);
}
/* output coordinates for animation */
fprintf(file3,"\n");
fprintf(file3,"link %f %f %f %f %f %f %f %f \\\n",
real(R[0]), imag(R[0]),
real(R[2]), imag(R[2]),
real(R[3]), imag(R[3]),
real(R[1]), imag(R[1]) );
fprintf(file3,"point trace %f %f \\\n", real(P), imag(P));
fprintf(file3,"polygon fill grey90 %f %f %f %f %f %f \\\n",
real(R[2]), imag(R[2]),
real(R[3]), imag(R[3]),
real(P), imag(P));
t3=dtheta3+t3;
fprintf(file3,"rectangle %f %f %f %f angle %f fill red \n", real(P), imag(P),rectx, recty,M_RAD2DEG(t3));
fprintf(file3,"\n");
}
fprintf(file3,"\n");
fclose(file2);
fclose(file3);
qanimate fourbar_animation.qnm
remove("fourbar_animation.qnm");
return 0;
}
]]>