6spsview.cpp
来自「此算法是牛顿欧拉法（在VC环境下）」· C++ 代码 · 共 1,058 行 · 第 1/2 页
CPP
1,058 行
dc.TextOut(250,60,"f (N)");
for(numi=-600;numi<=800;numi=numi+200)
{
dc.MoveTo(300,250-numi/5);
dc.LineTo(620,250-numi/5);
itoa(numi,str,10);
dc.TextOut(250,240-numi/5,str);
}
end=0.37699;
}
else
{
static char strt[7][5]={{'0'},{'0','.','0','5'},{'0','.','1','0'},{'0','.','1','5'},{'0','.','2','0'},{'0','.','2','5'},{'0','.','3','0'}};
dc.TextOut(565,404,"t (s)");
for(numi=0,i=0;numi<=0.3;numi=numi+0.05)
{
dc.MoveTo(300+numi*800,100);
dc.LineTo(300+numi*800,400);
dc.TextOut(290+numi*800,404,strt[i++]);
}
dc.TextOut(250,70,"f (N)");
for(numi=-750;numi<=750;numi=numi+250)
{
dc.MoveTo(300,250-numi/5);
dc.LineTo(540,250-numi/5);
itoa(numi,str,10);
dc.TextOut(250,240-numi/5,str);
}
end=0.2968667;
}
for(t=0;t<=end;t=t+0.0001)
{
if(cons==FALSE)
{
cal();
for(i=0;i<2;i++)
{
nt[i]=sp[i]/0.23;
}
nt[2]=0.222036/0.23;
bet=acos(nt[2]);
if(t<0.37699/2)
{
alp=-atan(nt[0]/nt[1]);
}
else
{
alp=pi-atan(nt[0]/nt[1]);
}
gam=-alp;
}
else
{
cbl();
alp=0;
bet=0;
gam=0;
}
tr[0][0]=cos(alp)*cos(gam)-sin(alp)*cos(bet)*sin(gam);
tr[0][1]=-cos(alp)*sin(gam)-sin(alp)*cos(bet)*cos(gam);
tr[1][1]=-sin(alp)*sin(gam)+cos(alp)*cos(bet)*cos(gam);
tr[1][2]=-sin(bet)*cos(alp);
tr[2][0]=sin(bet)*sin(gam);
tr[2][1]=sin(bet)*cos(gam);
tr[2][2]=cos(bet);
tr[0][2]=sin(alp)*sin(bet);
tr[1][0]=sin(alp)*cos(gam)+cos(alp)*cos(bet)*sin(gam);
for(i=0;i<6;i++)
{
for(j=0;j<3;j++)
{
tb[i][j]=0;
for(k=0;k<3;k++)
{
tb[i][j]=tb[i][j]+tr[j][k]*bb[i][k];
}
}
for(j=0;j<3;j++)
{
l[i][j]=tb[i][j]+sp[j]-aa[i][j];
}
l[i][3]=sqrt(pow(l[i][0],2)+pow(l[i][1],2)+pow(l[i][2],2));
for(j=0;j<3;j++)
{
el[i][j]=l[i][j]/l[i][3];
}
}
for(i=0;i<6;i++)
{
for(j=0;j<3;j++)
{
ja[i][j]=el[i][j];
}
cma=tb[i][0];cmb=tb[i][1];cmc=tb[i][2];
cmd=el[i][0];cme=el[i][1];cmf=el[i][2];
crossmul();
ja[i][3]=cmg;ja[i][4]=cmh;ja[i][5]=cmi;
}
for(i=0;i<6;i++)
{
vl[i][6]=0;
for(j=0;j<6;j++)
{
vl[i][6]=vl[i][6]+ja[i][j]*vp[j];
}
for(j=0;j<3;j++)
{
vl[i][j]=vl[i][6]*el[i][j];
}
}
for(i=0;i<6;i++)
{
cma=vp[3];cmb=vp[4];cmc=vp[5];
cmd=tb[i][0];cme=tb[i][1];cmf=tb[i][2];
crossmul();
cmd=cmg+vp[0];cme=cmh+vp[1];cmf=cmi+vp[2];
cma=el[i][0];cmb=el[i][1];cmc=el[i][2];
crossmul();
vl[i][3]=cmg/l[i][3];vl[i][4]=cmh/l[i][3];vl[i][5]=cmi/l[i][3];
}
for(i=0;i<6;i++)
{
cma=vp[3];cmb=vp[4];cmc=vp[5];
cmd=tb[i][0];cme=tb[i][1];cmf=tb[i][2];
crossmul();
cmd=cmg;cme=cmh;cmf=cmi;
crossmul();
cmd=ap[0]+cmg;
cme=ap[1]+cmh;
cmf=ap[2]+cmi;
cma=el[i][0];cmb=el[i][1];cmc=el[i][2];
crossmul();
al[i][3]=(cmg-2*vl[i][0]*vl[i][3])/l[i][3];
al[i][4]=(cmh-2*vl[i][1]*vl[i][4])/l[i][3];
al[i][5]=(cmi-2*vl[i][2]*vl[i][5])/l[i][3];
}
for(i=0;i<6;i++)
{
cma=al[i][3];cmb=al[i][4];cmc=al[i][5];
cmd=lt*el[i][0];cme=lt*el[i][1];cmf=lt*el[i][2];
crossmul();
at[i][0]=cmg;at[i][1]=cmh;at[i][2]=cmi;
cmd=(l[i][3]-lh)*el[i][0];
cme=(l[i][3]-lh)*el[i][1];
cmf=(l[i][3]-lh)*el[i][2];
crossmul();
ah[i][0]=cmg;ah[i][1]=cmh;ah[i][2]=cmi;
cma=vl[i][3];cmb=vl[i][4];cmc=vl[i][5];
cmd=vl[i][0]*el[i][0];
cme=vl[i][1]*el[i][1];
cmf=vl[i][2]*el[i][2];
crossmul();
ah[i][0]=ah[i][0]+2*cmg;
ah[i][1]=ah[i][1]+2*cmh;
ah[i][2]=ah[i][2]+2*cmi;
for(j=0;j<3;j++)
{
ft[i][j]=mt*at[i][j];
fh[i][j]=mh*ah[i][j];
}
}
for(i=0;i<3;i++)
{
fd[6][i]=0;md[6][i]=0;
}
for(i=0;i<6;i++)
{
for(j=0;j<3;j++)
{
ind[j]=ind[3]*el[i][j];
inu[j]=inu[3]*el[i][j];
ml[i][j]=(ind[j]+inu[j])*al[i][j+3];
}
cma=vl[i][3];cmb=vl[i][4];cmc=vl[i][5];
cmd=(ind[0]+inu[0])*vl[i][3];
cme=(ind[1]+inu[1])*vl[i][4];
cmf=(ind[2]+inu[2])*vl[i][5];
crossmul();
ml[i][0]=ml[i][0]+cmg;
ml[i][1]=ml[i][1]+cmh;
ml[i][2]=ml[i][2]+cmi;
cma=(l[i][3]-lh)*el[i][0];
cmb=(l[i][3]-lh)*el[i][1];
cmc=(l[i][3]-lh)*el[i][2];
cmd=fh[i][0];cme=fh[i][1];cmf=fh[i][2];
crossmul();
ml[i][0]=ml[i][0]+cmg;
ml[i][1]=ml[i][1]+cmh;
ml[i][2]=ml[i][2]+cmi;
cma=lt*el[i][0];cmb=lt*el[i][1];cmc=lt*el[i][2];
cmd=ft[i][0];cme=ft[i][1];cmf=ft[i][2];
crossmul();
ml[i][0]=ml[i][0]+cmg;
ml[i][1]=ml[i][1]+cmh;
ml[i][2]=ml[i][2]+cmi;
cma=el[i][0];cmb=el[i][1];cmc=el[i][2];
cmd=ml[i][0];cme=ml[i][1];cmf=ml[i][2];
crossmul();
fd[i][0]=cmg/l[i][3];
fd[i][1]=cmh/l[i][3];
fd[i][2]=cmi/l[i][3];
for(j=0;j<3;j++)
{
//fd[i][j]=-((fh[i][j]+mgh[j])*el[i][j]*(l[i][3]-lh)+(ft[i][j]+mgt[j])*el[i][j]*lt)/l[i][j];
fd[6][j]=fd[6][j]+fd[i][j];
}
cma=fd[i][0];cmb=fd[i][1];cmc=fd[i][2];
cmd=tb[i][0];cme=tb[i][1];cmf=tb[i][2];
crossmul();
md[6][0]=md[6][0]+cmg;
md[6][1]=md[6][1]+cmh;
md[6][2]=md[6][2]+cmi;
}
for(i=0;i<3;i++)
{
fp[i]=mp[i]*ap[i];
}
for(i=0;i<3;i++)
{
fto[i]=fe[i]+mgp[i]+fp[i]+fd[6][i];
}
fto[3]=fe[3]+md[6][0];
fto[4]=fe[4]+md[6][1];
fto[5]=fe[5]+md[6][2];
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
jt[i][j]=ja[j][i];
}
}
matrcal();
for(i=0;i<6;i++)
{
fr[i]=0;
for(j=0;j<6;j++)
{
fr[i]=fr[i]+ji[i][j]*fto[j];
}
if(i==0)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(255,0,0));
}
if(i==1)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(0,255,0));
}
if(i==2)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(0,0,255));
}
if(i==3)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(255,0,255));
}
if(i==4)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(0,255,255));
}
if(i==5)
{
dc.SetPixel(300+t*800,250-fr[i]/5,RGB(0,0,0));
}
}
}
}
void CMy6spsView::cal()
{
for(i=2;i<5;i++)
{
sp[i]=0;
vp[i]=0;
ap[i]=0;
}
sp[0]=0.06*cos(50*t/3);
sp[1]=0.06*sin(50*t/3);
sp[2]=0.39;
sp[5]=50*t/3;
vp[0]=-sin(50*t/3);
vp[1]=cos(50*t/3);
vp[5]=50/3;
ap[0]=-50*cos(50*t/3)/3;
ap[1]=-50*sin(50*t/3)/3;
ap[5]=0;
}
void CMy6spsView::cbl()
{
if(t<=0.1)
{
sp[0]=15*pow(t,2)/2-0.14765;
vp[0]=15*t;
ap[0]=15;
}
else if(t>0.1&&t<=0.1968667)
{
sp[0]=1.5*t-0.22265;
vp[0]=1.5;
ap[0]=0;
}
else
{
sp[0]=-7.5*pow(t,2)+4.453*t-0.513324;
vp[0]=4.453-15*t;
ap[0]=-15;
}
if(t<=0.0859904)
{
sp[1]=7.5*pow(t,2)-0.136;
vp[1]=15*t;
ap[1]=15;
}
else if(t<=0.2108764)
{
sp[1]=-0.1914576+1.28986*t;
vp[1]=1.28986;
ap[1]=0;
}
else
{
sp[1]=-0.524974+4.453*t-7.5*pow(t,2);
vp[1]=4.453-15*t;
ap[1]=-15;
}
if(t<=0.0881635)
{
sp[2]=7.5*pow(t,2)+0.31;
vp[2]=15*t;
ap[2]=15;
}
else if(t>0.0881635&&t<=0.2087035)
{
sp[2]=0.251704+1.32245*t;
vp[2]=1.32245;
ap[2]=0;
}
else
{
sp[2]=-0.074975+4.453*t-7.5*pow(t,2);
vp[2]=4.453-15*t;
ap[2]=-15;
}
for(i=3;i<6;i++)
{
sp[i]=0;
vp[i]=0;
ap[i]=0;
}
}
void CMy6spsView::crossmul()
{
cmg=cmb*cmf-cme*cmc;
cmh=cmc*cmd-cma*cmf;
cmi=cma*cme-cmb*cmd;
}
void CMy6spsView::matrcal()
{
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
ji[i][j]=0;
if(i==j)
{
ji[i][j]=1;
}
}
}
for(i=0;i<6;i++)
{
numa=jt[i][i];
for(j=i;j<6;j++)
{
jt[i][j]=jt[i][j]/numa;
}
for(j=0;j<6;j++)
{
ji[i][j]=ji[i][j]/numa;
}
for(j=i+1;j<6;j++)
{
numb=jt[j][i];
for(k=i;k<6;k++)
{
jt[j][k]=jt[j][k]-numb*jt[i][k];
}
for(k=0;k<6;k++)
{
ji[j][k]=ji[j][k]-numb*ji[i][k];
}
}
}
for(i=5;i>=0;i--)
{
for(j=i-1;j>=0;j--)
{
for(k=5;k>=0;k--)
{
ji[j][k]=ji[j][k]-ji[i][k]*jt[j][i];
}
}
}
for(i=0;i<6;i++)
{
ji[5][i]=ji[5][i]/jt[5][5];
}
}
void CMy6spsView::Onset()
{
// TODO: Add your command handler code here
setdia dlg;
judg=0;
Invalidate();
dlg.m_vir=0;
dlg.m_fx=85.7;
dlg.m_fy=41.8;
dlg.m_fz=93.4;
dlg.m_mx=8.08;
dlg.m_my=17.75;
dlg.m_mz=0.13;
int res;
res=MessageBox("请注意外部条件取值范围","警告",MB_ICONINFORMATION);
if(dlg.DoModal()==IDOK)
{
fe[0]=dlg.m_fx;
fe[1]=dlg.m_fy;
fe[2]=dlg.m_fz;
fe[3]=dlg.m_mx;
fe[4]=dlg.m_my;
fe[5]=dlg.m_mz;
if(dlg.m_vir==0)
{
cons=FALSE;
}
else
{
cons=TRUE;
}
if(cons==FALSE)
{
res=MessageBox("您选择了动姿态空间圆周运动并设置了外部条件，确认吗？","警告",MB_YESNO+MB_ICONQUESTION);
if(res==IDNO)
{
dlg.m_vir=0;
dlg.m_fx=85.7;
dlg.m_fy=41.8;
dlg.m_fz=93.4;
dlg.m_mx=8.08;
dlg.m_my=17.75;
dlg.m_mz=0.13;
if(dlg.DoModal()==IDOK)
{
fe[0]=dlg.m_fx;
fe[1]=dlg.m_fy;
fe[2]=dlg.m_fz;
fe[3]=dlg.m_mx;
fe[4]=dlg.m_my;
fe[5]=dlg.m_mz;
if(dlg.m_vir==0)
{
cons=FALSE;
}
else
{
cons=TRUE;
}
}
}
}
else
{
res=MessageBox("您选择了定姿态空间直线运动并设置了外部条件，确认吗？","注意",MB_YESNO+MB_ICONQUESTION);
if(res==IDNO)
{
dlg.m_vir=0;
dlg.m_fx=85.7;
dlg.m_fy=41.8;
dlg.m_fz=93.4;
dlg.m_mx=8.08;
dlg.m_my=17.75;
dlg.m_mz=0.13;
if(dlg.DoModal()==IDOK)
{
fe[0]=dlg.m_fx;
fe[1]=dlg.m_fy;
fe[2]=dlg.m_fz;
fe[3]=dlg.m_mx;
fe[4]=dlg.m_my;
fe[5]=dlg.m_mz;
if(dlg.m_vir==0)
{
cons=FALSE;
}
else
{
cons=TRUE;
}
}
}
}
}
}
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?