! EUC-jp ! 文字コードはEUC-jp ! 自由形式のfortranで書いています。 ! g77でコンパイルする場合は、 !g77 -ffree-form -o smv smv.f ! のようにコンパイルしてください。 ! gfortran でコンパイルする場合は文字コードをutf-8にしてください。 !折りたたみ4段のプログラムの分割数を増やせるようにした(08/11/20) !program daiya implicit real*8 (a-h, o-z) pi=2.d0*asin(1.d0) syu=1.8d-1 !円周 r=syu/2.d0/pi !半径。円周が36cmということ zure=0.5 !高さ方向のずれ m=6!周方向分割数 n=20!高さ方向分割数 h=9.d-2 !高さ dh=1.9d-3/n !ダイヤ三角1段ぶんの高さ(斜めになってるから三角形の高さではない) psi=asin(dh/(h/n)) !!! orih=h/n*cos(psi) !orih2=sqrt((h/n)**2-(dh)**2) nset=6*m+3*m*(2*n-1) !節点数 nyou=12*m*n !要素数 open(7,file='yousosuu.txt') write(7,*) nyou write(7,*) 2*m,2*n close(7) th=2.d0*pi/m !ダイヤ1つぶんの中心角 ph=(pi-th)/2. !多角形の1辺と中心を結ぶ三角形の底角 oh=pi/2.-th !多角形の1辺と中心を結ぶ三角形の底角 !ccccccccccccccccccccc sita=orih/cos(oh)*2 ue=orih*tan(oh)*2 b=((ue-sita)/2)+(syu/m) !ensyu=((b+sita)*m/2.)+((b-ue)*m/2.) hank=b/(2*sin(th/2)) !cccccccccccccccccccccc open(8,file='tubu4sub.obj') write(8,'("3DG1")') write(8,*) nset x1=hank*cos(ph)+orih/sin(th) y1=hank*sin(ph) x2=hank*cos(ph)-orih/tan(th) y2=hank*sin(ph)+orih uegawa=1. !同じ段で上側を向いているか下側を向いているかを正負で区別 do i=0,m-1 !円周方向に1回りずつ !xyz座標を出力 !上側右三角形の左下頂点、上、右下頂点を出して、 !thずつ座標変換で回転させる thi=th*real(i) th0=th*real(i-1) th1=th*real(i+1) j=1 !1段め前半 if(uegawa>0) then !上側を向いている場合、右三角形の上頂点だけz座標をdh下げる write(8,*) sin(thi)*y1, cos(thi)*y1, (j-1)*dh call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (sin(thi)*y1+x1d)/2,(cos(thi)*y1+y1d)/2,(j-1)*dh write(8,*) (sin(thi)*y1+x3d)/2,(cos(thi)*y1+y3d)/2,(2*j-1)*dh/2 write(8,*) (x1d+x3d)/2,(y1d+y3d)/2,(2*j-1)*dh/2 write(8,*) (x1d+(sin(th1)*y1))/2,(y1d+(cos(th1)*y1))/2,(2*j-1)*dh/2 write(8,*) x1d,y1d,(j-1)*dh else !下側を向いている場合、右三角形の左右下頂点をdh下げる。上頂点は下げない。 call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) ((cos(th0)*x1+sin(th0)*y1)+x3d)/2,& & ((-sin(th0)*x1+cos(th0)*y1)+y3d)/2, (j-1)*dh write(8,*) x3d,y3d,(j-1)*dh write(8,*) (sin(thi)*y1+x3d)/2,(cos(thi)*y1+y3d)/2,(2*j-1)*dh/2 write(8,*) (x1d+x3d)/2,(y1d+y3d)/2,(2*j-1)*dh/2 write(8,*) (x1d+sin(th1)*y1)/2,(y1d+cos(th1)*y1)/2,(2*j-1)*dh/2 write(8,*) (x3d+sin(th1)*y1)/2,(y3d+cos(th1)*y1)/2,(j-1)*dh endif uegawa=uegawa*(-1.) end do ! ! 1段め後半 j=1 thi=th*real(0) thm=th*real(m-1) call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (x3d+(cos(thm)*x1+sin(thm)*y1))/2,& & (y3d+(-sin(thm)*x1+cos(thm)*y1))/2, j*dh do i=1,m-1,2 !円周方向に1回りずつだけど1段めと節点を共有しないのは、 !下側右三角形の左下頂点とそれからth回転した上側中三角形の左右上頂点。 !この3頂点をthずつm/2回回転させればいいんでステップが2になってる。 thi=th*real(i) th0=th*real(i-1) !下側右三角形からth回転した上側中三角形の頂点用 th1=th*real(i+1) !下側右三角形からth回転した上側中三角形の頂点用 if(i/=m-1) then !周の開始から一つ前まで call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) x2d,y2d,j*dh write(8,*) (x2d+(sin(thi)*y1))/2,(y2d+(cos(thi)*y1))/2,j*dh write(8,*) sin(thi)*y1,cos(thi)*y1,j*dh write(8,*) ((sin(thi)*y1)+x1d)/2,((cos(thi)*y1)+y1d)/2,j*dh write(8,*) x1d,y1d,j*dh write(8,*) (x1d+x4d)/2,(y1d+y4d)/2,j*dh else !周の一番最後は、開始点の頂点とくっつくように call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) x2d,y2d,j*dh write(8,*) (x2d+(sin(thi)*y1))/2,(y2d+(cos(thi)*y1))/2,j*dh write(8,*) sin(thi)*y1,cos(thi)*y1,j*dh write(8,*) (sin(thi)*y1+x1d)/2,(cos(thi)*y1+y1d)/2,j*dh write(8,*) x1d,y1d,j*dh end if end do ! dansuu=1. do j=1,n-1 ! ! ! 2段め前半 uegawa=1. !同じ段で上側を向いているか下側を向いているかを正負で区別 do i=0,m-1 !円周方向に1回りずつ thi=th*real(i) th0=th*real(i-1) th1=th*real(i+1) if(uegawa>0) then !上側を向いている場合、右三角形の上頂点だけz座標をdh下げる call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (sin(thi)*y1+x3d)/2,(cos(thi)*y1+y3d)/2,(2*j+1)*dh/2. write(8,*) (x1d+x3d)/2,(y1d+y3d)/2,(2*j+1)*dh/2. write(8,*) (x1d+(sin(th1)*y1))/2,(y1d+(cos(th1)*y1))/2,(2*j+1)*dh/2. else !下側を向いている場合、右三角形の左右下頂点をdh下げる。上頂点は下げない。 call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (sin(thi)*y1+x3d)/2,(cos(thi)*y1+y3d)/2,(2*j+1)*dh/2. write(8,*) (x1d+x3d)/2,(y1d+y3d)/2,(2*j+1)*dh/2. write(8,*) (x1d+sin(th1)*y1)/2,(y1d+cos(th1)*y1)/2,(2*j+1)*dh/2. endif uegawa=uegawa*(-1.) end do ! ! ! if(dansuu>0) then ! ! ! 2段め後半 uegawa=1. !同じ段で上側を向いているか下側を向いているかを正負で区別 do i=0,m-1 !円周方向に1回りずつ thi=th*real(i) th0=th*real(i-1) th1=th*real(i+1) if(uegawa>0) then !上側を向いている場合、右三角形の上頂点だけz座標をdh下げる write(8,*) sin(thi)*y1, cos(thi)*y1, (j+1)*dh call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (sin(thi)*y1+x1d)/2,(cos(thi)*y1+y1d)/2,(j+1)*dh write(8,*) x1d,y1d,(j+1)*dh else !下側を向いている場合、右三角形の左右下頂点をdh下げる。上頂点は下げない。 call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) ((cos(th0)*x1+sin(th0)*y1)+x3d)/2,& & ((-sin(th0)*x1+cos(th0)*y1)+y3d)/2,(j+1)*dh write(8,*) x3d,y3d,(j+1)*dh write(8,*) (x3d+sin(th1)*y1)/2,(y3d+cos(th1)*y1)/2,(j+1)*dh endif uegawa=uegawa*(-1.) end do ! else ! thi=th*real(0) thm=th*real(m-1) call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) (x3d+(cos(thm)*x1+sin(thm)*y1))/2,& & (y3d+(-sin(thm)*x1+cos(thm)*y1))/2, (j+1)*dh do i=1,m-1,2 !円周方向に1回りずつだけど1段めと節点を共有しないのは、 !下側右三角形の左下頂点とそれからth回転した上側中三角形の左右上頂点。 !この3頂点をthずつm/2回回転させればいいんでステップが2になってる。 thi=th*real(i) th0=th*real(i-1) !下側右三角形からth回転した上側中三角形の頂点用 th1=th*real(i+1) !下側右三角形からth回転した上側中三角形の頂点用 if(i/=m-1) then !周の開始から一つ前まで call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) x2d,y2d,(j+1)*dh write(8,*) (x2d+(sin(thi)*y1))/2,(y2d+(cos(thi)*y1))/2,(j+1)*dh write(8,*) sin(thi)*y1,cos(thi)*y1,(j+1)*dh write(8,*) ((sin(thi)*y1)+x1d)/2,((cos(thi)*y1)+y1d)/2,(j+1)*dh write(8,*) x1d,y1d,(j+1)*dh write(8,*) (x1d+x4d)/2,(y1d+y4d)/2,(j+1)*dh else !周の一番最後は、開始点の頂点とくっつくように call zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) write(8,*) x2d,y2d,(j+1)*dh write(8,*) (x2d+(sin(thi)*y1))/2,(y2d+(cos(thi)*y1))/2,(j+1)*dh write(8,*) sin(thi)*y1,cos(thi)*y1,(j+1)*dh write(8,*) (sin(thi)*y1+x1d)/2,(cos(thi)*y1+y1d)/2,(j+1)*dh write(8,*) x1d,y1d,(j+1)*dh end if end do ! end if dansuu=dansuu*(-1.) end do ! ! ! !1周ずつ上側三形と下側三角形の頂点の節点を外から見て左回りに出力 !1段め前半 do i=0,2*m-2,2 !周方向に if(i/=2*m-2) then !周の開始から一つ前まで !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,i*3,i*3+1,i*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,i*3+2,i*3+1,i*3+3,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,i*3+1,i*3+5,i*3+3,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,i*3+3,i*3+5,i*3+4,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,i*3+5,i*3+6,i*3+4,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,i*3+4,i*3+6,i*3+8,' ','0xcccccc' else !周の一番最後は、開始点の頂点とくっつくように !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,i*3,i*3+1,i*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,i*3+2,i*3+1,i*3+3,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,i*3+1,i*3+5,i*3+3,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,i*3+3,i*3+5,i*3+4,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,i*3+5,0,i*3+4,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,i*3+4,0,2,' ','0xcccccc' end if end do ! !1段め後半 do i=0,2*m-2,2 !周方向に i2=i/2 if(i/=2*m-2) then !周の開始から一つ前まで !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,i*3+2,i*3+3,m*6+i2*3+1,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,m*6+i2*3+1,i*3+3,m*6+i2*3+2,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,i*3+3,i*3+4,m*6+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,m*6+i2*3+2,i*3+4,m*6+i2*3+3,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,i*3+4,i*3+8,m*6+i2*3+3,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,m*6+i2*3+3,i*3+8,m*6+i2*3+4,' ','0xaaaccc' else !周の一番最後は、開始点の頂点とくっつくように !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,i*3+2,i*3+3,m*6+i2*3+1,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,m*6+i2*3+1,i*3+3,m*6+i2*3+2,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,i*3+3,i*3+4,m*6+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,m*6+i2*3+2,i*3+4,m*6,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,i*3+4,2,m*6,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,m*6,2,m*6+1,' ','0xaaaccc' end if end do ! ! dansuu=1. do j=0,n-2 ! ! !2段め前半 do i=0,2*m-2,2 !周方向に i2=i/2 nh1=m*6*j if(i/=2*m-2) then !周の開始から一つ前まで !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh1+m*6+i2*3+1,nh1+m*6+i2*3+2,nh1+m*9+i2*3+1,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh1+m*9+i2*3+1,nh1+m*6+i2*3+2,nh1+m*9+i2*3+2,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh1+m*6+i2*3+2,nh1+m*6+i2*3+3,nh1+m*9+i2*3+2,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh1+m*9+i2*3+2,nh1+m*6+i2*3+3,nh1+m*9+i2*3+3,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh1+m*6+i2*3+3,nh1+m*6+i2*3+4,nh1+m*9+i2*3+3,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh1+m*9+i2*3+3,nh1+m*6+i2*3+4,nh1+m*9+i2*3+4,' ','0xaaaccc' else !周の一番最後は、開始点の頂点とくっつくように !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh1+m*6+i2*3+1,nh1+m*6+i2*3+2,nh1+m*9+i2*3+1,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh1+m*9+i2*3+1,nh1+m*6+i2*3+2,nh1+m*9+i2*3+2,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh1+m*6+i2*3+2,nh1+m*6,nh1+m*9+i2*3+2,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh1+m*9+i2*3+2,nh1+m*6,nh1+m*9,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh1+m*6,nh1+m*6+1,nh1+m*9,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh1+m*9,nh1+m*6+1,nh1+m*9+1,' ','0xaaaccc' end if end do ! if(dansuu>0) then ! !2段め後半 do i=0,2*m-2,2 !周方向に i2=i/2 nh2=12*m*j/2 if(i/=2*m-2) then !周の開始から一つ前まで !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh2+m*9+i2*3+1,nh2+m*9+i2*3+2,nh2+m*12+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*12+i2*3+2,nh2+m*9+i2*3+2,nh2+m*12+i2*3+3,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh2+m*9+i2*3+2,nh2+m*9+i2*3+3,nh2+m*12+i2*3+3,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*12+i2*3+3,nh2+m*9+i2*3+3,nh2+m*12+i2*3+4,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh2+m*9+i2*3+3,nh2+m*9+i2*3+4,nh2+m*12+i2*3+4,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh2+m*12+i2*3+4,nh2+m*9+i2*3+4,nh2+m*12+i2*3+5,' ','0xcccccc' else !周の一番最後は、開始点の頂点とくっつくように !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh2+m*9+i2*3+1,nh2+m*9+i2*3+2,nh2+m*12+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*12+i2*3+2,nh2+m*9+i2*3+2,nh2+m*12,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh2+m*9+i2*3+2,nh2+m*6+i2*3+3,nh2+m*12,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*12,nh2+m*6+i2*3+3,nh2+m*12+1,' ','0xaaaccc' !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh2+m*6+i2*3+3,nh2+m*6+i2*3+4,nh2+m*12+1,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh2+m*12+1,nh2+m*6+i2*3+4,nh2+m*12+2,' ','0xcccccc' end if end do ! else ! do i=0,2*m-2,2 !周方向に i2=i/2 nh2=12*m+(m*12*(j-1)/2) if(i/=2*m-2) then !周の開始から一つ前まで !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh2+m*3+i2*3,nh2+m*3+i2*3+1,nh2+m*6+i2*3+1,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh2+m*6+i2*3+1,nh2+m*3+i2*3+1,nh2+m*6+i2*3+2,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh2+m*3+i2*3+1,nh2+m*3+i2*3+2,nh2+m*6+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*6+i2*3+2,nh2+m*3+i2*3+2,nh2+m*6+i2*3+3,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh2+m*3+i2*3+2,nh2+m*3+i2*3+3,nh2+m*6+i2*3+3,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*6+i2*3+3,nh2+m*3+i2*3+3,nh2+m*6+i2*3+4,' ','0xaaaccc' else !周の一番最後は、開始点の頂点とくっつくように !上側左三角形の左下頂点、右下頂点、上 write(8,*) 3,nh2+m*3+i2*3,nh2+m*3+i2*3+1,nh2+m*6+i2*3+1,' ','0xcccccc' !上側中三角形の左上頂点、下、右上頂点 write(8,*) 3,nh2+m*6+i2*3+1,nh2+m*3+i2*3+1,nh2+m*6+i2*3+2,' ','0xcccccc' !上側右三角形の左下頂点、右下頂点、上頂点 write(8,*) 3,nh2+m*3+i2*3+1,nh2+m*3+i2*3+2,nh2+m*6+i2*3+2,' ','0xcccccc' !下側左三角形の左下頂点、上頂点、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*6+i2*3+2,nh2+m*3+i2*3+2,nh2+m*6,' ','0xaaaccc' !下側中三角形の左上頂点、右上頂点、下(下から見て左回り) write(8,*) 3,nh2+m*3+i2*3+2,nh2+m*3,nh2+m*6,' ','0xaaaccc' !下側右三角形の左下頂点、上、右下頂点(下から見て左回り) write(8,*) 3,nh2+m*6,nh2+m*3,nh2+m*6+1,' ','0xaaaccc' end if end do ! end if dansuu=dansuu*(-1.) end do write(8,'("#")') !daiya.objファイルから要素数を読み取るとき用 close(8) ! end ! ! ! subroutine zahen(thi,th0,th1,orih,x1,y1,x2,y2,x1d,y1d,x2d,y2d,x3d,y3d,x4d,y4d) implicit real*8 (a-h, o-z) x1d=cos(thi)*x1+sin(thi)*y1 y1d=-sin(thi)*x1+cos(thi)*y1 x2d=cos(th0)*x2+sin(th0)*y2 y2d=-sin(th0)*x2+cos(th0)*y2 x3d=cos(thi)*x2+sin(thi)*y2 y3d=-sin(thi)*x2+cos(thi)*y2 x4d=cos(th1)*x2+sin(th1)*y2 y4d=-sin(th1)*x2+cos(th1)*y2 return end