www.pudn.com > roll.rar > DYNAMIC.CPP
#include "stdafx.h"
#include "roll.h"
#include "bbroll.h"
#include "dynwnd.h"
#include "DynPara.h"
#include "dynamic.h"
#include "comnfun.h"
#include "intersec.h"
#include
int Roller::Calc_Roller(CFPoint roller[7],double m_r1,double m_r2,double& MASS,double& INERT)
{
double alfa,bita,alfag,alfad,RCENOUT;
double xx1,yy1,xx2,yy2,xx3,yy3,xx4,yy4;
double ttx,tty,rr2,AREA,MEMONT;
double& m_angle=theRoll.m_angle;
double& m_high=theRoll.m_high;
alfa = m_angle*M_PI/360.0;
bita = M_PI/teethnum;
if( alfa <= bita ) return 1;
RCENOUT = (m_r1+m_r2+m_high*sin(alfa));
rr2 = cos(alfa)*sin(bita)+sin(alfa)*(1.0-cos(bita));
RCENOUT = RCENOUT/rr2;
rmax=RCENOUT+m_r2;
rr2 = RCENOUT-m_high;
double m_tthick = rr2-m_r1; // m_tthick is the greatest inner circle radius
if( m_tthick <= hole ) return 1;
roller[6].x = -RCENOUT*sin(bita);
roller[6].y = RCENOUT*cos(bita);
roller[3].x = 0.0;
roller[3].y = rr2;
roller[2].x = -m_r1*cos(alfa);
roller[2].y = rr2-m_r1*sin(alfa);
roller[1].x = -roller[2].x;
roller[1].y = roller[2].y;
roller[4].x = roller[6].x+m_r2*cos(alfa);
roller[4].y = roller[6].y+m_r2*sin(alfa);
roller[0].x = -roller[4].x;
roller[0].y = roller[4].y;
alfa = 2.0*bita;
ttx = cos(alfa);
tty = sin(alfa);
roller[5].x = roller[0].x*ttx-roller[0].y*tty;
roller[5].y = roller[0].x*tty+roller[0].y*ttx;
ttx=0.5*(roller[0].x+roller[1].x);
tty=0.5*(roller[0].y+roller[1].y);
m_tthick=sqrt(ttx*ttx+tty*tty);
// Prepair the calculate mass and inertial
ttx = cos(bita);
tty = sin(bita);
xx1=roller[3].x*ttx+roller[3].y*tty;
yy1=-roller[3].x*tty+roller[3].y*ttx;
xx2=roller[3].x*ttx+(roller[3].y-m_r1)*tty;
yy2=-roller[3].x*tty+(roller[3].y-m_r1)*ttx;
xx3=roller[2].x*ttx+roller[2].y*tty;
yy3=-roller[2].x*tty+roller[2].y*ttx;
xx4=roller[4].x*ttx+roller[4].y*tty;
yy4=-roller[4].x*tty+roller[4].y*ttx;
alfag=theRoll.CalcAngle(roller[2].x,roller[2].y,roller[1].x,roller[1].y,roller[3].x,roller[3].y);
alfad=theRoll.CalcAngle(roller[4].x,roller[4].y,roller[5].x,roller[5].y,roller[6].x,roller[6].y);
// Calculate the mass of roller
AREA=0.5*xx2*yy2;
AREA=AREA+xx1*(yy3-yy2)+0.5*(xx1-xx3)*(yy1-yy3)-
0.25*m_r1*m_r1*alfag-0.5*(xx1-xx2)*(yy1-yy2);
AREA=AREA+0.5*(xx3+xx4)*(yy4-yy3);
AREA=AREA+0.25*alfad*m_r2*m_r2-0.5*(yy4-RCENOUT)*xx4;;
AREA=2.0*AREA*teethnum;
AREA=AREA-M_PI*hole*hole;
// Calculate the inertial of roller
MEMONT=xx2*yy2*(3*yy2*yy2+xx2*xx2)/12.0;
rr2=alfag*0.5;
MEMONT=MEMONT+xx1*(yy3-yy2)*(xx1*xx1/3+yy1*yy1+m_r1*m_r1)-2.0*yy1*
((xx3-xx1)*(xx3-xx1)*(xx3-xx1)-(xx2-xx1)*(xx2-xx1)*(xx2-xx1))/3.0+
0.5*(xx1*xx1+yy1*yy1+m_r1*m_r1)*(rr2*m_r1*m_r1+(yy3-yy1)*(xx3-xx1)-(yy2-yy1)*(xx2-xx1))+
xx1*yy1*((yy3-yy1)*(yy3-yy1)+(yy2-yy1)*(yy2-yy1));
MEMONT=MEMONT+(yy4-yy3)*(xx4+xx3)*(xx4*xx4+xx3*xx3)/12.0+
(xx3*yy4-xx4*yy3)*(yy4*yy4+yy3*yy4+yy3*yy3)/3.0+
(xx4-xx3)*(yy3+yy4)*(yy3*yy3+yy4*yy4)/4.0;
alfa=alfad*0.5;
MEMONT=MEMONT+0.25*(m_r2*m_r2+2.0*RCENOUT*RCENOUT)*(m_r2*m_r2*alfa-
xx4*(yy4-RCENOUT))+(yy4-RCENOUT)*xx4*xx4*xx4/6.0+
2.0*RCENOUT*xx4*xx4*xx4/3.0;
MEMONT=2.0*MEMONT*teethnum;
MEMONT=MEMONT-0.5*M_PI*hole*hole*hole*hole;
rr2=theRoll.m_Weight*theRoll.m_length;
MASS=rr2*AREA;
INERT=rr2*MEMONT;
return 0;
}
int Roller::SetRollerdata(int IsUnderFirst)
{
int i,j;
CFPoint roller[7];
double alfa=M_PI2/teethnum;
double cs,sn,tx,ty;
if(teeth) delete[] teeth;
teeth=new Tooth[teethnum];
double& m_r1=theRoll.m_r1;
double& m_r2=theRoll.m_r2;
if(Calc_Roller(roller,m_r1,m_r2,MASS,INERT)) return 1;
if(IsUnderFirst) {
teeth[0].x[0]=-roller[5].x;
teeth[0].y[0]=roller[5].y;
teeth[0].x[1]=roller[0].x;
teeth[0].y[1]=roller[0].y;
teeth[0].x[2]=-roller[6].x;
teeth[0].y[2]=roller[6].y;
teeth[0].x[3]=roller[1].x;
teeth[0].y[3]=roller[1].y;
teeth[0].x[4]=roller[2].x;
teeth[0].y[4]=roller[2].y;
teeth[0].x[5]=roller[3].x;
teeth[0].y[5]=roller[3].y;
}
else {
double alf=M_PI-M_PI/teethnum;
cs=cos(alf);
sn=sin(alf);
for(i=0;i<3;i++) {
tx=roller[4+i].x;
ty=roller[4+i].y;
teeth[0].x[i]=tx*cs-ty*sn;
teeth[0].y[i]=tx*sn+ty*cs;
}
teeth[0].x[2]=0.0;
tx=roller[1].x;
ty=roller[1].y;
teeth[0].x[4]=tx*cs-ty*sn;
teeth[0].y[4]=tx*sn+ty*cs;
tx=roller[2].x;
ty=roller[2].y;
teeth[0].x[3]=tx*cs-ty*sn;
teeth[0].y[3]=tx*sn+ty*cs;
tx=roller[3].x;
ty=roller[3].y;
teeth[0].x[5]=tx*cs-ty*sn;
teeth[0].y[5]=tx*sn+ty*cs;
for(i=3;i<6;i++)
teeth[0].x[i]=-teeth[0].x[i];
}
for(i=1;im_hWnd==NULL)
wnd->CreateEx(0,
strGeomWndClass,
"Dynamic Rollers Figure",
WS_OVERLAPPEDWINDOW,
0,0,540,450,pParentWnd->m_hWnd,
NULL,NULL);
wnd->ShowWindow(SW_SHOWNORMAL);//SW_SHOWMAXIMIZED);
wnd->DrawRollers( this );
}
void CDynamic::HideDynamicRollers()
{
if(wnd==NULL) return;
wnd->ShowWindow(SW_HIDE);
}
CDynamic::~CDynamic()
{
wnd->DestroyWindow();
if(wnd) delete wnd;
}
int CDynamic::SetTeethData(CDynPara& dynpara)
{
pDynPara = &dynpara;
Roller1.teethnum=dynpara.m_teethnum1;
Roller1.hole=dynpara.m_hole1;
Roller2.teethnum=dynpara.m_teethnum2;
Roller2.hole=dynpara.m_hole2;
friction=dynpara.m_friction;
if(Roller1.SetRollerdata(1)) return 1;
if(Roller2.SetRollerdata(0)) return 1;
disp1=velo1=acce1=0.0;
disp2=velo2=acce2=0.0;
disp3=velo3=acce3=0.0;
double t=0.0;
Roller1.SetX0(t);
Roller1.SetY0(t);
/* Roller2.SetX0(t);
t=Roller1.teeth[0].y[5]-theRoll.m_r1;
t=t+fabs(Roller2.teeth[0].y[2])+theRoll.m_r2;
Roller2.SetY0(t);
*/ double& l=dynpara.m_L1;
double& xp=dynpara.m_xp;
double& yp=dynpara.m_yp;
double p=sqrt(xp*xp+yp*yp);
t=Roller1.teeth[0].y[5]-theRoll.m_r1;
t+=fabs(Roller2.teeth[0].y[2])+theRoll.m_r2;
double alf=(l*l+p*p-t*t)/(2.0*l*p);
if(fabs(alf)>=1.0) return 1;
alf=acos(alf);
double bit=atan(fabs(yp/xp));
disp3=t=alf-bit;
alf=l*cos(t)-xp;
bit=l*sin(t)+yp;
Roller2.SetX0(alf);
Roller2.SetY0(bit);
t=atan(fabs(alf/bit));
Roller1.SetFai0(t);
Roller2.SetFai0(t);
double t1,t2;
alf=Roller1.teeth[1].x[0];
bit=Roller1.teeth[1].y[0];
t1=atan2(bit,alf);
alf=Roller1.teeth[0].x[4];
bit=Roller1.teeth[0].y[4];
t2=atan2(bit,alf);
cangle1=fabs(t2-t1);
alf=Roller1.teeth[0].x[3];
bit=Roller1.teeth[0].y[3];
t1=atan2(bit,alf);
cangle2=fabs(t2-t1);
alf=Roller1.teeth[0].x[1];
bit=Roller1.teeth[0].y[1];
t2=atan2(bit,alf);
cangle3=fabs(t2-t1);
alf=Roller1.teeth[0].x[0];
bit=Roller1.teeth[0].y[0];
t1=atan2(bit,alf);
cangle4=fabs(t2-t1);
cangle2=cangle2+cangle1;
cangle3=cangle3+cangle2;
cangle4=cangle4+cangle3;
return 0;
}
void CDynamic::CalcMaxContAngleNumber()
{
double x1=Roller1.GetX0();
double y1=Roller1.GetY0();
double x2=Roller2.GetX0();
double y2=Roller2.GetY0();
double d=sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
double s,t[2],xt[2],yt[2];
double& rmax=Roller1.rmax;
Circle_Circle_InterSection(x1,y1,rmax,x2,y2,Roller2.rmax,t);
xt[0]=rmax*cos(t[0]);
yt[0]=rmax*sin(t[0]);
xt[1]=rmax*cos(t[1]);
yt[1]=rmax*sin(t[1]);
s=sqrt((xt[0]-xt[1])*(xt[0]-xt[1])+(yt[0]-yt[1])*(yt[0]-yt[1]));
s=0.5*s;
double angle=asin(s/rmax);
contrange=int(angle*Roller1.teethnum/M_PI2)+1;
}
void CDynamic::DetermineOrgTension(double b,double R0,double R,double v,double a,double& tension)
{
double inertia=RevInertia+0.5*M_PI*density*(R*R-R0*R0)*(R*R+R0*R0)/b;
tension=(resistmoment+inertia*(a+b*v*v/(M_PI2*R*R))/R)/R-density*v*v;
}
void CDynamic::DetermineTension(double x0,double y0,double r,double angle,
double& Tension,double omiga,double epsln)
{
double s=omiga*omiga;
double t=friction*friction;
double H=1.0+t;
double D=r*density*H;
double E=1.0-t;
double F=density*r*r*(epsln/friction-s);
double st=D*(((gravity-s*y0+epsln*x0)*E-2.0*s*friction*x0-2.0*epsln*friction*y0)*sin(angle)-
((s*x0+epsln*y0)*E+2.0*friction*gravity-2.0*s*friction*y0+2.0*epsln*x0)*cos(angle))-F;
Tension=st+constant*exp(friction*angle);
}
void CDynamic::DetermineConstant(double x0,double y0,double r,double angle,
double Tension,double omiga,double epsln)
{
double s=omiga*omiga;
double t=friction*friction;
double H=1.0+t;
double D=r*density*H;
double E=1.0-t;
double F=density*r*r*(epsln/friction-s);
double st=D*(((gravity-s*y0+epsln*x0)*E-2.0*s*friction*x0-2.0*epsln*friction*y0)*sin(angle)-
((s*x0+epsln*y0)*E+2.0*friction*gravity-2.0*s*friction*y0+2.0*epsln*x0)*cos(angle))-F;
constant=(Tension-st)/exp(friction*angle);
}
void CDynamic::CalcForceXY(double x0,double y0,double r,double angle,
double& fx,double& fy,double omiga,double epsln)
{
double t1,t2;
double t=0.5*r*density/(1.0+friction*friction);
double s1=gravity-omiga*omiga*y0+epsln*x0;
double s2=omiga*omiga*x0+epsln*y0;
t1=friction*s1+s2;
t2=s1-friction*s2;
fx=-t*(t1*(sin(2.0*angle)+2.0*angle)+t2*cos(2.0*angle));
t1=s1-friction*s2;
t2=friction*s1+s2;
fy=t*(t1*(2.0*angle-sin(2.0*angle))+t2*cos(2.0*angle));
t=r*r*density*epsln/friction;
fx=fx-t*sin(angle);
fy=fy+t*cos(angle);
s1=constant*exp(friction*angle)/(1.0+friction*friction);
t=sin(angle)+friction*cos(angle);
fx=fx+s1*t;
t=friction*sin(angle)-cos(angle);
fy=fy+s1*t;
}
void CDynamic::CalcForceM(double x0,double y0,double r,double angle,
double& m,double omiga,double epsln)
{
double p=omiga*omiga;
double q=friction*friction;
double s=1.0+q;
double t=1.0-q;
double d=angle*density*r/s;
double mm=gravity*x0*t+epsln*x0*x0*t+2.0*p*q*x0*y0+epsln*y0*y0*s-2.0*p*friction*x0*x0-2.0*epsln*friction*x0*y0;
m=d*mm;
d=0.5*density*r*sin(2.0*angle)/s;
mm=gravity*x0*s-2.0*gravity*friction*y0-2.0*p*x0*y0-epsln*x0*x0*t-2.0*epsln*friction*x0*y0-epsln*y0*y0*t+2.0*epsln*r*r*s;
m=m-d*mm;
d=0.5*density*r*cos(2.0*angle)/s;
mm=gravity*y0*t+2.0*epsln*x0*y0-p*y0*y0*t-p*x0*x0*s-2.0*p*friction*x0*y0-2.0*epsln*friction*y0*y0;
m=m+d*mm;
d=density*r*r*friction/(3.0*s);
mm=(gravity-p*y0+epsln*x0-p*friction*y0)*cos(3.0*angle)+(friction*gravity-p*friction*y0+epsln*friction*x0+p*x0+epsln*y0)*sin(3.0*angle);
m=m-d*mm;
d=density*r*r/s;
mm=(friction*(gravity-p*y0+2.0*epsln*x0-friction*(2.0*epsln*y0+p*x0))+epsln*x0/friction-epsln*y0)*cos(angle);
mm=mm-(q*(gravity-p*y0+2.0*epsln*x0)+p*friction*x0+epsln*x0-epsln*y0/friction)*sin(angle);
m=m+d*mm;
d=constant*exp(friction*angle)/s;
mm=(2.0*friction*x0-s*y0)*sin(angle)-t*x0*cos(angle);
m=m+d*mm;
m=m+constant*friction*r*exp(friction*angle)*(2.0*sin(2.0*angle)+friction*cos(2.0*angle))/(4.0+q);
}
void CDynamic::CalcPerForceMoment(double x0,double y0,double r,double starta,double enda,
double Tension,double& fx,double& fy,double& moment,double omiga,double epsln)
{
double fx1,fy1,fx2,fy2,m1,m2;
DetermineConstant(x0,y0,r,starta,Tension,omiga,epsln);
CalcForceXY(x0,y0,r,starta,fx1,fy1,omiga,epsln);
CalcForceM(x0,y0,r,starta,m1,omiga,epsln);
CalcForceXY(x0,y0,r,enda,fx2,fy2,omiga,epsln);
CalcForceM(x0,y0,r,enda,m2,omiga,epsln);
fx=fx2-fx1;
fy=fy2-fy1;
moment=m2-m1;
}
void CDynamic::CalcTotalForceMoment(double& Fx,double& Fy,double& Moment)
{
int i,j1,j2;
int& teethnum1=Roller1.teethnum;
int& teethnum2=Roller2.teethnum;
double& rmax=Roller1.rmax;
CalcMaxContAngleNumber();
DetermineOrgTension(thickpaper,RevR0,RevR,velo2*rmax,acce2*rmax,tension);
for(i=contrange;i>=0;i--)
{
j1=contnum1-i;
if(j1<0) j1=teethnum1-j1;
j2=i+contnum2;
if(j2>=teethnum2) j2=j2-teethnum2;
}
}
void CDynamic::DeterContNormal(double x1[7],double y1[7],double a1[4],
double x2[7],double y2[7],double a2[4],
int ret[16],double x[16],double y[16],
double& nx1,double& ny1,double& nx2,double& ny2)
{
int i,j,k;
double t,tx,ty;
for(i=0;i<16;i++)
if(ret[i]==1) {
j=i/4; // below roller segment
k=i%4; // upper roller segment
switch( j ) {
case 0:
tx=x[i]-x1[2];
ty=y[i]-y1[2];
t=atan2(ty,tx);
nx1=cos(t);
ny1=sin(t);
break;
case 1:
t=a1[1];
nx1=cos(t);
ny1=sin(t);
break;
case 2:
tx=x[i]-x1[5];
ty=y[i]-y1[5];
t=atan2(ty,tx);
nx1=-cos(t);
ny1=-sin(t);
break;
case 3:
t=a1[3];
nx1=-cos(t);
ny1=-sin(t);
break;
}
switch( k ) {
case 0:
tx=x[i]-x2[2];
ty=y[i]-y2[2];
t=atan2(ty,tx);
nx2=-cos(t);
ny2=-sin(t);
break;
case 1:
t=a2[2];
nx2=cos(t);
ny2=sin(t);
break;
case 2:
tx=x[i]-x2[5];
ty=y[i]-y2[5];
t=atan2(ty,tx);
nx2=cos(t);
ny2=sin(t);
break;
case 3:
t=a2[3];
nx2=cos(t);
ny2=sin(t);
break;
}
return;
}
}
void CDynamic::DetermineToothContact(double x1[7],double y1[7],double a1[4],
double x2[7],double y2[7],double a2[4],
int ret[16],double x[16],double y[16])
{
double& rd=theRoll.m_r2;
double& rg=theRoll.m_r1;
ret[0] =Arc_Arc_Int_S (x1[2],y1[2],a1[0],a1[1],rd,x2[2],y2[2],a2[0],a2[1],rg,x[0],y[0]);
ret[1] =Arc_Line_Int_S(x1[2],y1[2],a1[0],a1[1],rd,x2[1],y2[1],x2[3],y2[3],x[1],y[1]);
ret[2] =Arc_Arc_Int_S (x1[2],y1[2],a1[0],a1[1],rd,x2[5],y2[5],a2[3],a2[2],rd,x[2],y[2]);
ret[3] =Arc_Line_Int_S(x1[2],y1[2],a1[0],a1[1],rd,x2[4],y2[4],x2[6],y2[6],x[3],y[3]);
ret[4] =Line_Arc_Int_S (x1[1],y1[1],x1[3],y1[3],x2[2],y2[2],a2[0],a2[1],rg,x[4],y[4]);
ret[5] =Line_Line_Int_S(x1[1],y1[1],x1[3],y1[3],x2[1],y2[1],x2[3],y2[3],x[5],y[5]);
ret[6] =Line_Arc_Int_S (x1[1],y1[1],x1[3],y1[3],x2[5],y2[5],a2[3],a2[2],rd,x[6],y[6]);
ret[7] =Line_Line_Int_S(x1[1],y1[1],x1[3],y1[3],x2[4],y2[4],x2[6],y2[6],x[7],y[7]);
ret[8] =Arc_Arc_Int_S (x1[5],y1[5],a1[3],a1[2],rg,x2[2],y2[2],a2[0],a2[1],rg,x[8],y[8]);
ret[9] =Arc_Line_Int_S(x1[5],y1[5],a1[3],a1[2],rg,x2[1],y2[1],x2[3],y2[3],x[9],y[9]);
ret[10]=Arc_Arc_Int_S (x1[5],y1[5],a1[3],a1[2],rg,x2[5],y2[5],a2[3],a2[2],rd,x[10],y[10]);
ret[11]=Arc_Line_Int_S(x1[5],y1[5],a1[3],a1[2],rg,x2[4],y2[4],x2[6],y2[6],x[11],y[11]);
ret[12]=Line_Arc_Int_S (x1[4],y1[4],x1[6],y1[6],x2[2],y2[2],a2[0],a2[1],rg,x[12],y[12]);
ret[13]=Line_Line_Int_S(x1[4],y1[4],x1[6],y1[6],x2[1],y2[1],x2[3],y2[3],x[13],y[13]);
ret[14]=Line_Arc_Int_S (x1[4],y1[4],x1[6],y1[6],x2[5],y2[5],a2[3],a2[2],rd,x[14],y[14]);
ret[15]=Line_Line_Int_S(x1[4],y1[4],x1[6],y1[6],x2[4],y2[4],x2[6],y2[6],x[15],y[15]);
}
int CDynamic::DetermineContact(double time,double& cx,double& cy,double& nx1,double& ny1,double& nx2,double& ny2)
{
int i1,i2,j1,j2;
int& teethnum1=Roller1.teethnum;
int& teethnum2=Roller2.teethnum;
double x1[7],y1[7],a1[4];
double x2[7],y2[7],a2[4];
CalcMaxContAngleNumber();
int i1cont=-1,i2cont=-1;
cx=cy=nx1=nx2=ny1=ny2=0.0;
int i1from,i1end,i2from,i2end;
if(IsCounterClockWise) {
i1from=-contrange;
i1end=contrange;
i2from=contrange;
i2end=-contrange;
}
else {
i1from=contrange;
i1end=-contrange;
i2from=-contrange;
i2end=contrange;
}
int Inc1,Inc2;
Inc1=i1end>i1from?1:0;
Inc2=i2end>i2from?1:0;
for(i2=i2from;(Inc2?i2<=i2end:i2>=i2end);(Inc2?i2++:i2--)) {
j2=i2+contnum2;
if(j2<0) j2=teethnum2+j2;
if(j2>=teethnum2) j2=j2-teethnum2;
x2[0]=Roller2.teeth[j2].x[4];
y2[0]=Roller2.teeth[j2].y[4];
x2[1]=Roller2.teeth[j2].x[3];
y2[1]=Roller2.teeth[j2].y[3];
x2[2]=Roller2.teeth[j2].x[5];
y2[2]=Roller2.teeth[j2].y[5];
x2[3]=Roller2.teeth[j2].x[1];
y2[3]=Roller2.teeth[j2].y[1];
x2[4]=Roller2.teeth[j2].x[0];
y2[4]=Roller2.teeth[j2].y[0];
x2[5]=Roller2.teeth[j2].x[2];
y2[5]=Roller2.teeth[j2].y[2];
// NOTICE convert the tooth into ONE coordinate system
int i=j2-1;
if(i<0) i=teethnum2-1;
x2[6]=Roller2.teeth[i].x[4];
y2[6]=Roller2.teeth[i].y[4];
double t=Roller2.GetFai()+Roller2.GetFai0();
double cs=cos(t);
double sn=sin(t);
for(i=0;i<7;i++) {
t=x2[i]*cs-y2[i]*sn;
y2[i]=x2[i]*sn+y2[i]*cs;
x2[i]=t;
}
cs=Roller2.GetX0();
sn=Roller2.GetY0();
for(i=0;i<7;i++) {
x2[i]+=cs;
y2[i]+=sn;
}
a2[0]=atan2(y2[0]-y2[2],x2[0]-x2[2]);
a2[1]=atan2(y2[1]-y2[2],x2[1]-x2[2]);
a2[2]=atan2(y2[3]-y2[5],x2[3]-x2[5]);
a2[3]=atan2(y2[4]-y2[5],x2[4]-x2[5]);
AdjustAngle(a2[0],a2[1]);
AdjustAngle(a2[3],a2[2]);
int ret[16];
double x[16],y[16];
for(i1=i1from;(Inc1?i1<=i1end:i1>=i1end);(Inc1?i1++:i1--)) {
j1=i1+contnum1;
if(j1<0) j1=teethnum1+j1;
if(j1>=teethnum1) j1=j1-teethnum1;
for(i=0;i<16;i++)
x[i]=y[i]=0.0;
i=j1+1;
if(i>=Roller1.teethnum) i=0;
x1[6]=Roller1.teeth[i].x[0];
y1[6]=Roller1.teeth[i].y[0];
for(i=0;i<6;i++) {
x1[i]=Roller1.teeth[j1].x[i];
y1[i]=Roller1.teeth[j1].y[i];
}
t=Roller1.GetFai()+Roller1.GetFai0();
cs=cos(t);
sn=sin(t);
for(i=0;i<7;i++) {
t=x1[i]*cs-y1[i]*sn;
y1[i]=x1[i]*sn+y1[i]*cs;
x1[i]=t;
}
a1[0]=atan2(y1[0]-y1[2],x1[0]-x1[2]);
a1[1]=atan2(y1[1]-y1[2],x1[1]-x1[2]);
a1[2]=atan2(y1[3]-y1[5],x1[3]-x1[5]);
a1[3]=atan2(y1[4]-y1[5],x1[4]-x1[5]);
AdjustAngle(a1[0],a1[1]);
AdjustAngle(a1[3],a1[2]);
DetermineToothContact(x1,y1,a1,x2,y2,a2,ret,x,y);
for(i=0;i<16;i++)
if(ret[i]==-1)
return 1;// happen interface;
else if(ret[i]==1) {
if(i1cont<0) i1cont=j1;
if(i2cont<0) i2cont=j2;
if(i1cont>=0) {
cx=x[i];
cy=y[i];
DeterContNormal(x1,y1,a1,x2,y2,a2,ret,x,y,nx1,ny1,nx2,ny2);
}
}
}
}
contnum1=i1cont;
contnum2=i2cont;
return 0;
}
void CDynamic::DetermineInitContFM(double cx,double cy,double nx1,double ny1,double nx2,double ny2,double& M1,double& M2,double& M3)
{
double FPress=pDynPara->m_force;
double x20=Roller2.GetX0();
double y20=Roller2.GetY0();
double& lg=pDynPara->m_L1;
FPress=(1.0+pDynPara->m_L2/lg)*FPress;
double t,x2,y2;
double RelVx,RelVy;
double tanx1,tany1,tanx2,tany2;
x2=cx-x20;
y2=cy-y20;
RelVx=-lg*sin(disp3)*velo3-y2*velo2;
RelVy=lg*cos(disp3)*velo3+x2*velo2;
RelVx=RelVx+cy*velo1;
RelVy=RelVy-cx*velo1;
tanx1=-ny1;
tany1=nx1;
tanx2=-ny2;
tany2=nx2;
t=RelVx*tanx2+RelVy*tany2;
if(t>0.0) {
tanx2=-tanx2;
tany2=-tany2;
}
t=RelVx*tanx1+RelVy*tany1;
if(t<0.0) {
tanx1=-tanx1;
tany1=-tany1;
}
double NormPress,FFriction;
double MG2=Roller2.MASS*gravity;
NormPress=FPress+MG2*cos(disp3);
NormPress=NormPress/(ny2*cos(disp3)-nx2*sin(disp3));
FFriction=friction*fabs(NormPress);
M1=FFriction*(cx*tany1-cy*tanx1);
M1=M1+NormPress*(cx*ny1-cy*nx1);
M2=FFriction*(x2*tany2-y2*tanx2);
M2=M2+NormPress*(x2*ny2-y2*nx2);
RelVx=NormPress*nx2+FFriction*tanx2;
RelVy=NormPress*ny2+FFriction*tany2;
t=RelVy*cos(disp3)-RelVx*sin(disp3);
M3=t-FPress-MG2*cos(disp3);
M3=M3*lg;
}
void CDynamic::DetermineContFM(double cx,double cy,double nx1,double ny1,double nx2,double ny2,double& M1,double& M2,double& M3)
{
double FPress=pDynPara->m_force;
double x20=Roller2.GetX0();
double y20=Roller2.GetY0();
double& lg=pDynPara->m_L1;
FPress=(1.0+pDynPara->m_L2/lg)*FPress;
double t,x2,y2;
double RelVx,RelVy;
double tanx1,tany1,tanx2,tany2;
x2=cx-x20;
y2=cy-y20;
RelVx=-lg*sin(disp3)*velo3-y2*velo2;
RelVy=lg*cos(disp3)*velo3+x2*velo2;
RelVx=RelVx+cy*velo1;
RelVy=RelVy-cx*velo1;
tanx1=-ny1;
tany1=nx1;
tanx2=-ny2;
tany2=nx2;
t=RelVx*tanx2+RelVy*tany2;
if(t>0.0) {
tanx2=-tanx2;
tany2=-tany2;
}
t=RelVx*tanx1+RelVy*tany1;
if(t<0.0) {
tanx1=-tanx1;
tany1=-tany1;
}
double NormPress,FFriction;
double&mass2=Roller2.MASS;
double MG2=mass2*gravity;
NormPress=FPress+MG2*cos(disp3)+mass2*lg*acce3;
NormPress=NormPress/((ny2+friction*tany2)*cos(disp3)-(nx2+friction*tanx2)*sin(disp3));
FFriction=friction*fabs(NormPress);
M1=FFriction*(cx*tany1-cy*tanx1);
M1=M1+NormPress*(cx*ny1-cy*nx1)-Roller1.INERT*acce1;
M2=FFriction*(x2*tany2-y2*tanx2);
M2=M2+NormPress*(x2*ny2-y2*nx2)-Roller2.INERT*acce2;
RelVx=NormPress*nx2+FFriction*tanx2;
RelVy=NormPress*ny2+FFriction*tany2;
t=RelVy*cos(disp3)-RelVx*sin(disp3);
M3=t-FPress-MG2*cos(disp3);
M3=M3*lg-mass2*lg*lg*acce3;
}
void CDynamic::Runge_Kutta(int n,double x,double* y0,double s,double* y)
{
int i;
double k1[20],k2[20],k3[20],k4[20],t;
ControlFunction(x,y0,k1);
t=0.5*s;
for(i=0;im_L1;
xp=-pDynPara->m_xp;
yp=pDynPara->m_yp;
INERT1=Roller1.INERT;
INERT2=Roller2.INERT;
INERT3=lg*lg*Roller2.MASS;
MOMENT1=pDynPara->m_moment;
MOMENT2=-pDynPara->m_zmoment;
MOMENT30=-lg*pDynPara->m_force;
InitRAngle=fabs(atan(Roller1.teeth[1].x[0]/Roller1.teeth[1].y[0]));
double s=0.01;
double t=0.0;
double fai0[6];
double sangle=M_PI2/Roller1.teethnum;
disp1=Roller1.GetFai0()+0.5*M_PI;
disp2=1.5*M_PI+Roller2.GetFai0();
fai0[0]=disp1;
fai0[1]=disp2;
fai0[2]=disp3;
fai0[3]=0.0;
fai0[4]=0.0;
fai0[5]=0.0;
int i;
double fai[6],nf[6];
FILE *fpp=fopen("Ddyn.dat","w");
if(fpp==NULL) return 1;
disp1=disp1+pDynPara->m_numteeth*sangle;
while(fai0[0]M_PI2)
angle-=M_PI2;
while(angle>sangle) {
contnum1--;
angle-=sangle;
}
while(contnum1<0)
contnum1=contnum1+teethnum;
double x[6],y[6];
int iprev=contnum1+1;
if(iprev>=teethnum) iprev=0;
x[0]=Roller1.teeth[iprev].x[0];
y[0]=Roller1.teeth[iprev].y[0];
x[1]=Roller1.teeth[contnum1].x[4];
y[1]=Roller1.teeth[contnum1].y[4];
x[2]=Roller1.teeth[contnum1].x[3];
y[2]=Roller1.teeth[contnum1].y[3];
x[3]=Roller1.teeth[contnum1].x[5];
y[3]=Roller1.teeth[contnum1].y[5];
x[4]=Roller1.teeth[contnum1].x[1];
y[4]=Roller1.teeth[contnum1].y[1];
x[5]=Roller1.teeth[contnum1].x[2];
y[5]=Roller1.teeth[contnum1].y[2];
double t,cs,sn;
t=fai-0.5*M_PI;
cs=cos(t);
sn=sin(t);
for(int i=0;i<6;i++) {
t=x[i];
x[i]=t*cs-y[i]*sn;
y[i]=t*sn+y[i]*cs;
}
double tangle;
if(angle=tangle)
ARCGSEG(FixAngle,x[3],y[3],rg,r,drdfai);
// else {
// }
}
else if(angle