phd-scripts/Unpublished/RigidSurface/rsurfu_surf.for

103 lines
2.3 KiB
Text
Raw Normal View History

2024-05-13 19:50:21 +00:00
subroutine rsurfu(h,p,tgt,dnds,x1,time,u,ciname,slname,msname,
1 noel,node,lclose)
include 'aba_param.inc'
c
character*80 ciname,slname,msname
dimension p(3),tgt(3,2),dnds(3,2),x1(3,2),time(2)
parameter(zero=0.d0,one=1.d0)
c get cylinder radius
stime=time(2)
if(msname(1:5)=='INNER')then
ri=0.4d0
drdt=one
if(stime<=one)then
radius=ri+drdt*stime
elseif(stime<=2.d0)then
radius=ri+drdt*(2.d0-stime)
else
radius=ri
endif
else
ri=1.8d0
drdt=-one
if(stime>=3.05d0)then
radius=ri+drdt*(stime-3.05d0)
else
radius=ri
endif
endif
c initialize variables
do k1=1,2
do k2=1,3
tgt(k2,k1) = zero
dnds(k2,k1) = zero
p(k2) = zero
enddo
enddo
c coordinates of point on deforming body
x = x1(1,1)
y = x1(2,1)
z = x1(3,1)
c get point on rigid cylinder
x2=x1(1,1)+x1(1,3)
y2=x1(2,1)+x1(2,3)
z2=x1(3,1)+x1(3,3)
dx=x2-x
dy=y2-y
dr=sqrt(dx*dx+dy*dy)
D=x*y2-x2*y
disc=radius*radius*dr*dr-D*D
if(disc==zero)then
xcor=D*dy/(dr*dr)
ycor=-D*dx/(dr*dr)
elseif(disc<zero)then
xcor=1.e6
ycor=1.e6
print *,'warning - perpendicular slave master normals'
else
x1cor=(d*dy+sign(one,dy)*dx*disc)/(dr*dr)
x2cor=(d*dy-sign(one,dy)*dx*disc)/(dr*dr)
y1cor=(-d*dx+abs(dy)*disc)/(dr*dr)
y2cor=(-d*dx-abs(dy)*disc)/(dr*dr)
dx1=x1cor-x
dx2=x2cor-x2
dy1=y1cor-y
dy2=y2cor-y2
d1=sqrt(dx1*dx1+dy1*dy1)
d2=sqrt(dx2*dx2+dy2*dy2)
z1cor=x1(3,3)*d1+z
z2cor=x1(3,3)*d2+z
dz1=z1cor-z
dz2=z2cor-z2
dist1=sqrt(dx1*dx1+dy1*dy1+dz1*dz1)
dist2=sqrt(dx2*dx2+dy2*dy2+dz2*dz2)
if(dist1<dist2)then
p(1) = x1cor
p(2) = y1cor
p(3) = z1cor
else
p(1) = x2cor
p(2) = y2cor
p(3) = z2cor
endif
endif
c project point onto unit radius cylinder
r = ( x*x + y*y )**(0.5d0)
x = p(1)/radius
y = p(2)/radius
c get unit tangents
tgt(1,1) = -y
tgt(2,1) = x
tgt(3,2) = one
c get local curvatures
dnds(1,1) = -y / radius
dnds(2,1) = x / radius
c get surface penetration depth
if(msname(1:5)=='INNER')then
h = radius - r
else
h = r - radius
endif
return
end