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