C語言程序設計之平面連桿機構解析法完整版

#include<stdio.h>
#include<math.h>

double RRR(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double x2,double x2d,double x2dd,double y2,double y2d,double y2dd,double r1,double r2,double M,double *theta1,double *theta1d,double *theta1dd,double *theta2,double *theta2d,double *theta2dd,double *x3,double *x3d,double *x3dd,double *y3,double *y3d,double *y3dd,double *v,double *gamma,double *a,double *beta)
{
	double d1,d,phi,alpha,s1,v1,a1,A,B,C,D; //統一定義函數內變量 
	d1 = pow(x2-x1,2)+pow(y2-y1,2);  //分步計算d 
	d = sqrt(d1);
	phi = atan((y2-y1)/(x2-x1));  //計算方位角Phi 
	s1 = (pow(r1,2)+d-pow(r2,2))/(2*r1*d);  //分步計算內角alpha1 
	alpha = acos(s1);
	*theta1 = phi+M*alpha;  //計算構件1的方位角 theta1
	*x3 = x1+r1*cos(*theta1);  //計算節點3的座標 
	*y3 = y1+r1*sin(*theta1);
	*theta2 = atan((*y3-y2)/(*x3-x2));  //計算構件2的方位角  theta2
	A = (*y3-y2)*(*x3-x1)-(*y3-y1)*(*x3-x2);  //簡化計算 
	*theta1d = ((x2d-x1d)*(*x3-x2)+(y2d-y1d)*(*y3-y2))/(A);  //計算theta1的一階導數 
	*theta2d = ((x2d-x1d)*(*x3-x1)+(y2d-y1d)*(*y3-y1))/(A);  //計算theta2的一階導數
	*x3d = x1d - *theta1d*(*y3-y1);  // 計算x3的一階導數 
	*y3d = y1d + *theta1d*(*x3-x1);  // 計算y3的一階導數
	v1 = pow(*x3d,2)+(*y3d,2);  //分步計算速度v 
	*v = sqrt(v1);
	*gamma = atan((*y3d)/(*x3d));   //計算速度方位角gamma 
	B = x2dd-x1dd+pow(*theta1d,2)*(*x3-x1)-pow(*theta2d,2)*(*x3-x2);  //簡化計算 
	C = y2dd-y1dd+pow(*theta1d,2)*(*y3-y1)-pow(*theta2d,2)*(*y3-y2);  //簡化計算
	D = (*x3-x1)*(*y3-y2)-(*x3-x2)*(*y3-y1);  //簡化計算
	*theta1dd = ((B)*(*x3-x2)+(C)*(*y3-y2))/(D);  //計算theta1的二階導數 
	*theta2dd = ((B)*(*x3-x1)+(C)*(*y3-y1))/(D);  //計算theta2的二階導數
	*x3dd = x1dd-pow(*theta1d,2)*(*x3-x1)-*theta1dd*(*y3-y1);  // 計算x3的二階導數 
	*y3dd = y1dd-pow(*theta1d,2)*(*y3-y1)+*theta1dd*(*x3-x1);  // 計算y3的二階導數
	a1 = pow(*x3dd,2)+(*y3dd,2);  //分步計算加速度a 
	*a = sqrt(a1);
	*beta = atan((*y3dd)/(*x3dd));   //計算加速度方位角beta 
}

double RRP(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double x2,double x2d,double x2dd,double y2,double y2d,double y2dd,double phi,double phid,double phidd,double r,double M,double *x3,double *x3d,double *x3dd,double *y3,double *y3d,double *y3dd,double *v,double *gamma,double *a,double *beta,double *theta,double *thetad,double *thetadd,double *s,double *sd,double *sdd) 
{
	double d,d1,v1,E,F,K,L,O,N,A,B;  //統一定義函數內變量 
	d1 = pow(x2-x1,2)+pow(y2-y1,2);  //分步計算d 
	d = sqrt(d1); 
	E = 2*((x2-x1)*cos(phi)+(y2-y1)*sin(phi));  //簡化計算 
	F = d*d-r*r;
	*s = abs(-E+M*sqrt(E*E-4*F))/2;  //求滑塊與參考點的距離 
	*x3 = x2+*s*cos(phi);  //求滑塊的座標 
	*y3 = y2+*s*sin(phi);
	*theta = atan((*y3-y1)/(*x3-x1));  //求杆的方位角 
	K = x2d-x1d-*s*phid*sin(phi);  //簡化計算 
	L = y2d-y1d-*s*phid*cos(phi);
	A = (*y3-y1)*sin(phi)+(*x3-x1)*cos(phi);
	*thetad = (L*cos(phi)-K*sin(phi))/(A);  //求杆的方位角的一階導數 
	*sd = (-L*(*y3-y1)-K*(*x3-x1))/(A);  //求滑塊與參考點的距離的一階導數 
	*x3d = x1d-*thetad*r*sin(*theta);  //求滑塊的座標的一階導數 
	*y3d = y1d-*thetad*r*cos(*theta);
	v1 = pow(*x3d,2)+pow(*y3d,2);  //分步求滑塊的速度 
	*v = sqrt(v1);
	*gamma = atan((*y3d)/(*x3d));  //求速度方位角 
	O = (x2dd-x1dd+pow(*thetad,2)*(*x3-x1)-pow(phid,2)*(*s)*cos(phi)-2*phid*(*s)*sin(phi)-phidd*(*y3-y2));  //簡化計算 
	N = (y2dd-y1dd+pow(*thetad,2)*(*y3-y1)-pow(phid,2)*(*s)*sin(phi)-2*phid*(*s)*cos(phi)-phidd*(*x3-x2));
	B = (*y3-y1)*sin(phi)+(*x3-x1)*cos(phi);
	*thetadd = (-O*sin(phi)+N*cos(phi))/(B);  //求杆的方位角的二階導數
	*sdd = (-O*(*x3-x1)-N*(*y3-y1))/(B);  //求滑塊與參考點的距離的二階導數
	*x3dd = x1dd-pow(*thetad,2)*r*cos(*theta)-*thetadd*r*sin(*theta);  //求滑塊的座標的二階導數
	*y3dd = y1dd-pow(*thetad,2)*r*sin(*theta)+*thetadd*r*cos(*theta);
	*a = sqrt((*x3dd)*(*x3dd)+(*y3dd)*(*y3dd));  //求滑塊加速度 
	*beta = atan((*y3d)/(*x3dd));  //求加速度方位角 
 } 
 
 double QB(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double phi,double phid,double phidd,double *x2,double *x2d,double *x2dd,double *y2,double *y2d,double *y2dd,double *v,double *gamma,double *a,double *beta) 
 {
 	*x2 = x1+cos(phi);  //求遠端點的座標 
 	*y2 = y1+sin(phi);
 	*x2d = x1d-sin(phi)*phid;  //求遠端點座標的一階導數 
 	*y2d = y1d+cos(phi)*phid;
 	*v = sqrt((*x2d)*(*x2d)+(*y2d)*(*y2d));  //求遠端點速度 
	*gamma = atan((*y2d)/(*x2d));  //速度方位角 
	*x2dd = x1dd-cos(phi)*phid*phid-sin(phi)*phidd;  //求遠端點座標的二階導數 
	*y2dd = y1dd-sin(phi)*phid*phid+cos(phi)*phidd;
	*a = sqrt((*x2dd)*(*x2dd)+(*y2dd)*(*y2dd));  //求遠端點加速度 
	*beta = atan((*y2dd)/(*x2dd));   //求加速度方位角 
 }
 
 int main() 
{
	double s,sd,sdd,O,M,theta1,theta1d,theta1dd,theta2,theta2d,theta2dd,theta3,theta3d,theta3dd,theta4,theta4d,theta4dd,xa,xad,xadd,ya,yad,yadd,xc,xcd,xcdd,yc,ycd,ycdd,xb,xbd,xbdd,yb,ybd,ybdd,xd,xdd,xddd,yd,ydd,yddd,xe,xed,xedd,ye,yed,yedd,xf,xfd,xfdd,yf,yfd,yfdd,l1,l2,l3,l31,l32,l4,vb,gammab,ab,betab,vc,gammac,ac,betac,vd,gammad,ad,betad,ve,gammae,ae,betae,vf,gammaf,af,betaf,d23;
	int i;
	printf("請輸入模式係數M：\n");
	scanf("%lf",&M);
	printf("請輸入原動件的初始角（rad）和角速度(rad/s)，並用逗號分隔：\n");
	scanf("%lf,%lf",&theta1,&theta1d);
	printf("請輸入固定鉸鏈A的座標，並用逗號分隔：\n");
	scanf("%lf,%lf",&xa,&ya);
	printf("請輸入固定鉸鏈D的座標，並用逗號分隔：\n");
	scanf("%lf,%lf",&xd,&yd);
	xdd = 0,xddd = 0;
	ydd = 0,yddd = 0;
	printf("請輸入AB,BC,CD,EC,EF的長度(mm)，並用逗號分隔：\n");
	scanf("%lf,%lf,%lf,%lf,%lf",&l1,&l2,&l31,&l32,&l4);
	l3=l31+l32;
	xad = 0,xadd = 0;
	yad = 0,yadd = 0;
	theta1dd = 0;
	QB(xa,xad,xadd,ya,yad,yadd,theta1,theta1d,theta1dd,&xb,&xbd,&xbdd,&yb,&ybd,&ybdd,&vb,&gammab,&ab,&betab);
	RRR(xb,xbd,xbdd,yb,ybd,ybdd,xd,xdd,xddd,yd,ydd,yddd,l2,l31,M,&theta2,&theta2d,&theta2dd,&theta3,&theta3d,&theta3dd,&xc,&xcd,&xcdd,&yc,&ycd,&ycdd,&vc,&gammac,&ac,&betac);
	QB(xd,xdd,xdd,yd,ydd,yddd,theta3,theta3d,theta3dd,&xe,&xed,&xedd,&ye,&yed,&yedd,&ve,&gammae,&ae,&betae);
	RRP(xe,xed,xedd,ye,yed,yedd,xd,xdd,xddd,yd,ydd,yddd,O,O,O,l4,M,&xf,&xfd,&xfdd,&yf,&yfd,&yfdd,&vf,&gammaf,&af,&betaf,&theta4,&theta4d,&theta4dd,&s,&sd,&sdd);
	printf("B點的座標爲（%lf,%lf），速度爲%lf mm/s，速度方位角爲%lf rad，加速度爲%lf rad/(s^2)，加速度方位角爲%lf rad。\n",xb,yb,vb,gammab,ab,betab);
	printf("C點的座標爲（%lf,%lf），速度爲%lf mm/s，速度方位角爲%lf rad，加速度爲%lf mm/s，加速度方位角爲%lf rad。\n",xc,yc,vc,gammac,ac,betac);
	printf("E點的座標爲（%lf,%lf），速度爲%lf mm/s，速度方位角爲%lf rad，加速度爲%lf mm/s，加速度方位角爲%lf rad。\n",xe,ye,ve,gammae,ae,betae);
	printf("F點的座標爲（%lf,%lf），速度爲%lf mm/s，速度方位角爲%lf rad，加速度爲%lf mm/s，加速度方位角爲%lf rad。\n",xf,yf,vf,gammaf,af,betaf);
	printf("構件2的方位角爲%lf rad，方位角速度爲%lf rad/s，方位角加速度爲%lf rad/(s^2)\n",theta2,theta2d,theta2dd);
	printf("構件3的方位角爲%lf rad，方位角速度爲%lf rad/s，方位角加速度爲%lf rad/(s^2)\n",theta3,theta3d,theta3dd);
	printf("構件4的方位角爲%lf rad，方位角速度爲%lf rad/s，方位角加速度爲%lf rad/(s^2)\n",theta4,theta4d,theta4dd);
	return 0;
}