c

This program simulates motion of a missile when ejected horizontally

with an initila velocity

The object does not have any self propulsion

The object is geometrically cylindrical shaped

with hemospherical nose and flat end

parameter (nmax=10000)
real m,ixx,iyy,izz,ixy,iyz,izx,iyx,izy,ixz
real k_vt,k_pt,k_rt,k_vw,k_vq,k_wr,k_wp,k_uva,
&

k_uvd,k_up,k_ura,k_urd,k_pq,k_qr
real m_ut,m_wt,m_qt,m_uqa,m_uqd,m_uwa,m_uwd,m_vr,

&

m_vp,m_rr,m_pp,m_rp,m_wq,m_uu,m_wwa,m_wwd,

&

m_st,m_qt1,m_qt2
real n_ut,n_pt,n_rt,n_uva,n_uvd,n_up,n_ura,n_urd,

&

n_wp,n_vq,n_pq,n_qr,n_vw,n_vv

defineing arrays

dimension a(6,6),uu(6),udt(6),alfa(3),ro(3),tol(3),du(6),dalfa(6),
&

fs(6),fn(6),fs1(6),fs2(6),rs(3),rn(3),rs1(3),rs2(3),

&

ts(3),tn(3),ts1(3),ts2(3),un(6),alfn(3),ron(3),anew(6,6)

dimension tm(nmax),x(nmax),z(nmax),theta(nmax),ux(nmax)
dimension xp(8),zp(8),xgg(8),zgg(8)

they are like global variables

common/space1/m,xg,yg,zg,ixx,iyy,izz,ixy,iyz,izx,iyx,izy,ixz,

&

vol,xb,yb,zb,rho,grav,by
common/spacex/x_ut,x_wt,x_qt,x_wq,x_qq,x_vr,x_rr,x_rp,x_uq,

&

x_v,x_vv,x_w,x_ww
common/spacey/y_vt,y_pt,y_rt,y_ura,y_urd,y_wp,y_pq,y_up,

&

y_wr,y_qr,y_uv,y_vv
common/spacez/z_ut,z_wt,z_qt,z_uqa,z_uqd,z_vp,z_rp,z_pp,

&

z_wq,z_qq,z_uw,z_ww,z_st
common/spacek/k_vt,k_pt,k_rt,k_vw,k_vq,k_wr,k_wp,k_uva,

&

k_uvd,k_up,k_ura,k_urd,k_pq,k_qr
common/spacem/m_ut,m_wt,m_qt,m_uqa,m_uqd,m_uwa,m_uwd,m_vr,

&

m_vp,m_rr,m_pp,m_rp,m_wq,m_uu,m_wwa,m_wwd,m_st
common/spacen/n_ut,n_pt,n_rt,n_uva,n_uvd,n_up,n_ura,n_urd,

&

n_wp,n_vq,n_pq,n_qr,n_vw,n_vv

common/space2/al,br
common/space3/ai(6,6)
common/spacep/x_uu,x_u

open(8,file='out_track')
open(20,file='out_plot')

setting standard variables

pi

= 3.14159265358979323846

factor = pi/180.
rho = 1.0217
grav = 9.806

Initialization
uu(1) = 0.

uu(2) = 0.
uu(3) = 0.
uu(4) = 0.
uu(5) = 0.
uu(6) = 0.
udt(1) = 0.
udt(2) = 0.
udt(3) = 0.
udt(4) = 0.
udt(5) = 0.
udt(6) = 0.
alfa(1) = 0.
alfa(2) = 0.
alfa(3) = 0.
ro(1) = 0.
ro(2) = 0.
ro(3) = 0.

do i = 1,6
do j = 1,6
a(i,j) = 0.
anew(i,j) = 0.
ai(i,j) = 0.
enddo
enddo

READING DATA HERE

mass and gravity properties

al = 7.20

br = 0.533
write(*,*)' enter length of the object in m'
read(*,*) al
write(*,*)' enter diameter of the object in m'
read(*,*) br

alb is distance of cb from nose

alb = 3.644

rad = br/2.
vol1 = pi*rad**2*(al-rad)
vol2 = (2./3.)*pi*(rad**3)
vol = pi*rad**2*(al-rad) + (2./3.)*pi*rad**3

amom1 = vol1 * (rad + 0.5*(al - rad) )

amom2 = vol2 * (5./8.)*rad

alb = (amom1 + amom2)/vol

write(*,*)' vol=',vol,' alb=',alb

by = vol * rho
m = by

write(*,*)' additional wt. over buoyancy (in kg) ?'

read(*,*) deltam
deltam = deltam/1000.
am = m + deltam

write(*,*)' xg in mm'
read(*,*) xg
xg = xg/1000.
yg = 0
write(*,*)' zg in mm'
read(*,*) zg
zg = zg/1000.

write(*,*)' ** The estimates of damping coefficients are based'

write(*,*)' on slender body non-viscous theory **'
write(*,*)' The real fluid effects will change these somewhat'
write(*,*)' Here we use a user-input factor to model the change'
write(*,*)' Only the pitch coeff. m_w and m_q are modified'
write(*,*)' We modify by multiplying the non-added mass part in'
write(*,*)' these terms by a factor'
write(*,*)''
write(*,*)' enter the factor for m_w (typically between 1 to 2)'
read(*,*) fact1
write(*,*)' enter the factor for m_q (typically between 1 to 2)'
read(*,*) fact2

xb = 0
yb = 0
zb = 0

alg = alb - xg
al1 = al - alg
al1 = al - alb
al2 = al - al1 - rad

xt = -al1 + xg
amt = rho * pi * rad**2

d1 = 0.5*rho*al
d2 = d1*al
d3 = d2*al
d4 = d3*al
d5 = d4*al

ixx = am*(0.34*br)**2
iyy = am*(0.2839*al)**2
izz = iyy
ixy = 0
iyz = 0
izx = 0
iyx = ixy
izy = izy
ixz = izx

read here resistance data

x_u = 0.000 * d2
x_uu = 0.0006 * d2

dm = m/d3
adm =am/d3

read the added masses

x_ut = - rho*(2./3.)*pi*rad**3
x_wt = 0.
x_qt = 0.
dx_ut = x_ut / d3
dx_wt = x_wt / d3
dx_qt = x_qt / d4

y_vt = - rho*vol
y_pt = 0.0
y_rt1 = 0.5*rho*pi*rad**2*(al2**2-al1**2)
al22 = al2 + (3./8.)*rad
y_rt2 = (2./3.)*rho*pi*rad**3 * al22
y_rt = -(y_rt1 + y_rt2)
dy_vt = y_vt / d3
dy_pt = y_pt / d4
dy_rt = y_rt / d4

z_wt = y_vt
z_qt = - y_rt
dz_wt = z_wt / d3
dz_qt = z_qt / d4

dk_pt = -0.000000
dk_rt = 0.0
k_pt = dk_pt * d5
k_rt = dk_rt * d5

m_qt1 = (1./3.)*rho*pi*rad**2*(al2**3 + al1**3)

al22 = al2 + (3./8.)*rad

m_qt2 = (2./3.)*rho*pi*rad**3 * al22**2

m_qt = -(m_qt1 + m_qt2)
dm_qt = m_qt / d5

n_rt = m_qt
dn_rt = n_rt / d5

z_ut = x_wt
k_vt = y_pt
m_ut = x_qt
m_wt = z_qt
n_vt = y_rt
n_pt = k_rt

dz_ut = dx_wt
dk_vt = dy_pt
dm_ut = dx_qt
dm_wt = dz_qt
dn_vt = dy_rt
dn_pt = dk_rt
c
c

write(*,*)' non-dimensiomnal inertial coefficients'

write(*,*)' x_ut=',dx_ut,' x_wt=',dx_wt,' x_qt=',dx_qt

write(*,*)' y_vt=',dy_vt,' y_pt=',dy_pt,' y_rt=',dy_rt

write(*,*)' z_ut=',dz_ut,' z_wt=',dz_wt,' z_qt=',dz_qt

write(*,*)' k_vt=',dk_vt,' k_pt=',dk_pt,' k_rt=',dk_rt

write(*,*)' m_ut=',dm_ut,' m_wt=',dm_wt,' m_qt=',dm_qt

write(*,*)' n_vt=',dn_vt,' n_pt=',dn_pt,' n_rt=',dn_rt

x-coefficients

added mass
x_wq = z_wt
x_qq = z_qt
x_vr = -y_vt
x_rr = -y_rt
x_rp = -y_pt
x_uq = x_wt

x_wq = 0.
x_qq = 0.
x_vr = 0.
x_rr = 0.
x_rp = 0.
x_uq = 0.

damping
x_v = 0.0
x_vv = 0.0
x_w = 0.0
x_ww = 0.0

y force coefficients

added mass
y_ura = x_ut
y_ura = 0.
y_wp = -z_wt
y_pq = -z_qt

y_up = -x_wt
y_wr = x_wt
y_qr = x_qt

damping
y_uv = -amt
y_vv = -0.0

* d2

y_urd = 0.00 * d3
y_urd = - xt * amt

z force coefficients

added mass
z_uqa = -x_ut
z_uqa = 0.
z_vp = y_vt
z_rp = y_rt
z_pp = y_pt
z_wq = -x_wt
z_qq = -x_qt

damping

c--------------c

estimating z coefficients

c-------------------z_st = 0.0 * d2
z_uw = - amt
z_ww = 0.0

* d2

z_uqd = + xt * amt

k-moment coefficients

added mass
k_wr = -(y_rt + z_qt)
k_wp = -y_pt
k_uva = x_wt
k_ura = -x_qt
k_pq = k_rt
k_qr = n_rt - m_qt
k_vw = z_wt - y_vt
k_vq = y_rt + z_qt

damping
k_uvd = 0.0
k_up = 0.0
k_urd = 0.0

m-moment equations

added mass
m_uwa = x_ut - z_wt
m_uqa = -z_qt
m_vr = y_pt
m_vp = -y_rt
m_rr = k_rt
m_pp = -k_rt
m_rp = k_pt - n_rt
m_wq = x_qt
m_uu = -x_wt
m_wwa = x_wt

damping

c------------------c

estimatng m coeff

c
m_st = 0.0 * d3
m_uwd = xt * amt
m_uwd = fact1 * m_uwd
m_wwd = 0. * d3
m_uqd = - xt**2 * amt
m_uqd = fact2 * m_uqd
c------------

n_moment equations

added mass
n_uva = y_vt - x_ut
n_up = x_qt + y_pt
n_ura = y_rt
n_wp = z_qt
n_vq = -(x_qt+y_pt)
n_pq = m_qt - k_pt
n_qr = -k_rt
n_vw = -x_wt

damping
n_uvd = -xt * amt
n_vv = 0.00 * d3
n_urd = - xt**2 * amt

dy_v = y_uv/d2

dn_r = (n_ura+n_urd)/d4
dn_v = (n_uva+n_uvd)/d3
dy_r = (y_ura+y_urd)/d3

dz_w = z_uw/d2
dm_q = (m_uqa+m_uqd)/d4
dm_w = (m_uwa+m_uwd)/d3
dz_q = (z_uqa+z_uqd)/d3

write(*,*)' zw =',dz_w,' mq =',dm_q

write(*,*)' mw =',dm_w,' zq =',dz_q
write(*,*)'m =',dm,' am=',adm,' zq+m=',dz_q+adm
write(*,*)' mq/(m+zq)=',dm_q/(adm+dz_q),' mw/zw=',dm_w/dz_w
c_in = dz_w * dm_q - dm_w * (dz_q+adm)
write(*,*)' c =',c_in

m = am

**********************

setting the [a] matrix

a(1,1) = m - x_ut
a(1,3) =

- x_wt

a(1,5) = m*zg - x_qt

a(1,6) = -m*yg

a(2,2) = m - y_vt
a(2,4) = -m*zg - y_pt
a(2,6) = m*xg - y_rt

a(3,1) =

- z_ut

a(3,3) = m - z_wt
a(3,4) = m*yg
a(3,5) = -m*xg - z_qt

a(4,2) = -m*zg - k_vt

a(4,3) = m*yg
a(4,4) = ixx - k_pt
a(4,5) = - ixy
a(4,6) = - ixz - k_rt

a(5,1) = m*zg - m_ut

a(5,3) = -m*xg - m_wt
a(5,4) = - iyx
a(5,5) = iyy - m_qt
a(5,6) = - iyz

a(6,1) = -m*yg
a(6,2) = m*xg - n_vt
a(6,4) = - izx - n_pt
a(6,5) = - izy
a(6,6) = izz - n_rt

invert [a] , inv[a] = [ai]

do 203 i = 1,6
do 203 j = 1,6
203 anew(i,j) = a(i,j)
call invert(6,anew,ai)

tol(1) = al/10000
tol(2) = tol(1)
tol(3) = tol(1)

write(*,*)' command speed in m/s'

read(*,*) uc
ucc = uc
c

write(*,*)'Time upto which simulation continues in sec'

read(*,*) tmax
theta0 = 0.
theta0 = theta0 * pi/180.

write(*,*)'Initial depth in m'

read(*,*) r03
r03 = 0

write(*,*)'Delta t in sec'

read(*,*) delt

write(*,*)' delt =',delt

delt = 1./100.

uu(1) = uc
uu(1) = uc * cos(theta0)
uu(3) = uc * sin(theta0)
alfa(1) = alfa(1) * factor
alfa(2) = alfa(2) * factor
alfa(3) = alfa(3) * factor

start calculation for time step

n=0
t=0
alfa(2) = theta0
ro(3) = r03

TIME LOOP STARTS HERE

100
c

continue
write(*,*)' n=',n,' time=',t
time = t

call subroutine to determine forces

call accln(time,uc,uu,alfa,fs,rs,ts)

Integrating motion equaions using ABM predictor-correrctor scheme

predictor step
if ( n .eq. 0 ) go to 10
do j = 1,6
fs2(j) = fs1(j)
enddo
do j = 1,3
rs2(j) = rs1(j)
ts2(j) = ts1(j)
enddo

10

continue

do j = 1,6
fs1(j) = fs(j)
enddo
do j = 1,3
rs1(j) = rs(j)
ts1(j) = ts(j)
enddo

if ( n .eq. 0 ) then
do j = 1,6
un(j) = uu(j) + delt*fs1(j)
enddo
do j = 1,3
alfn(j) = alfa(j) + delt*rs1(j)
ron(j) = ro(j) + delt*ts1(j)
enddo
else
do j = 1,6
un(j) = uu(j) + 0.5*delt*(3*fs1(j)-fs2(j))
enddo
do j = 1,3
alfn(j) = alfa(j) + 0.5*delt*(3*rs1(j)-rs2(j))
ron(j) = ro(j) + 0.5*delt*(3*ts1(j)-ts2(j))
enddo
endif

do j = 1,6
du(j) = un(j)
enddo

do j = 1,3
dalfa(j) = alfn(j)
enddo

upgrade time for correrctor step

tt = t + delt
time = tt

call routine for compute force for corrector step

call accln(time,uc,du,dalfa,fn,rn,tn)

corrector steps
if ( n .eq. 0 ) then
do j = 1,6
un(j) = uu(j) + 0.5*delt*(fn(j) + fs1(j))
enddo
do j = 1,3
alfn(j) = alfa(j) + 0.5*delt*(rn(j) + rs1(j))
ron(j) = ro(j) + 0.5*delt*(tn(j) + ts1(j))
enddo
else
do j = 1,6
un(j) = uu(j)+delt*(5.*fn(j)+8*fs1(j)-fs2(j))/12
enddo
do j = 1,3
alfn(j) = alfa(j)+delt*(5.*rn(j)+8*rs1(j)-rs2(j))/12
ron(j)=ro(j)+delt*(5.*tn(j)+8*ts1(j)-ts2(j))/12
enddo

endif

n= n+1
t = t + delt
do 71 j = 1,6
71

uu(j) = un(j)

do 72 j = 1,3
alfa(j) = alfn(j)
72

ro(j) = ron(j)

set some limit for simulation to stop if some criteria exceeds

eg., here if pitch angle exceeds 30 deg., we stop simulatiobn

fac = 180/pi

write(8,88) n,t,ro(1),ro(3),alfa(2)/factor,uu(1)
tm(n) = t
x(n) = ro(1)
z(n) = - ro(3)
theta(n) = alfa(2)
ux(n) = uu(1)

if (abs(alfa(2)) .ge. pi/2) go to 505

c

if (t .gt. tmax) go to 505

if (n .ge. nmax) go to 505

go to 100

88

format(i8,f10.4,4f10.4)

505 continue

fac = 180/pi
ntot = n
write(8,*) ntot
do n = 1,n
write(8,88) n,tm(n),x(n),z(n),theta(n)/factor,ux(n)
enddo

write(*,*)' total time steps for output is :',ntot

for plotting

xp(1) = +0.5*al - rad

zp(1) = -rad

xp(2) = xp(1) + 0.866025*rad

zp(2) = -0.5*rad

xp(3) = +0.5 *al

zp(3) = 0.

xp(4) = xp(1) + 0.866025*rad

zp(4) = 0.5*rad

xp(5) = 0.5*al - rad

zp(5) = rad

xp(6) = -0.5*al
zp(6) = rad

xp(7) = -0.5*al
zp(7) = -rad

xp(8) = xp(1)
zp(8) = zp(1)

ndstep = 5
write(*,*)' after which steps plots needed'
read(*,*) ndstep

c
k=0
c

initial configuration
i= 0
tinit = 0.
write(20,*) k,i,tinit
do j = 1,8
write(20,*) j,xp(j),zp(j)
enddo

do i = 1,ntot
ii = i/ndstep
if ((i - ii*ndstep) .eq. 0 .or. i .eq. ntot ) then
k = k+1
x0 = x(i)

z0 = z(i)
th = theta(i)
cth = cos(th)
sth = sin(th)
write(20,*) k,i,tm(i)
do j = 1,8
xgg(j) = x0 + xp(j)*cth - zp(j)*sth
zgg(j) = z0 + xp(j)*sth + zp(j)*cth
write(20,*) j,xgg(j),zgg(j)
enddo
endif
enddo

stop
end

subroutine accln(time,uc,vv,beta,f,rr,t)

subroutuine that calculates various force, and does some computation

for the transformation

real m,ixx,iyy,izz,ixy,iyz,izx,iyx,izy,ixz
real k_vt,k_pt,k_rt,k_vw,k_vq,k_wr,k_wp,k_uva,
&

k_uvd,k_up,k_ura,k_urd,k_pq,k_qr
real m_ut,m_wt,m_qt,m_uqa,m_uqd,m_uwa,m_uwd,m_vr,

&

m_vp,m_rr,m_pp,m_rp,m_wq,m_uu,m_wwa,m_wwd,

&

m_st

real n_ut,n_pt,n_rt,n_uva,n_uvd,n_up,n_ura,n_urd,
&

n_wp,n_vq,n_pq,n_qr,n_vw,n_vv

common/space1/m,xg,yg,zg,ixx,iyy,izz,ixy,iyz,izx,iyx,izy,ixz,
&

vol,xb,yb,zb,rho,grav,by
common/spacex/x_ut,x_wt,x_qt,x_wq,x_qq,x_vr,x_rr,x_rp,x_uq,

&

x_v,x_vv,x_w,x_ww
common/spacey/y_vt,y_pt,y_rt,y_ura,y_urd,y_wp,y_pq,y_up,

&

y_wr,y_qr,y_uv,y_vv
common/spacez/z_ut,z_wt,z_qt,z_uqa,z_uqd,z_vp,z_rp,z_pp,

&

z_wq,z_qq,z_uw,z_ww,z_st
common/spacek/k_vt,k_pt,k_rt,k_vw,k_vq,k_wr,k_wp,k_uva,

&

k_uvd,k_up,k_ura,k_urd,k_pq,k_qr
common/spacem/m_ut,m_wt,m_qt,m_uqa,m_uqd,m_uwa,m_uwd,m_vr,

&

m_vp,m_rr,m_pp,m_rp,m_wq,m_uu,m_wwa,m_wwd,m_st
common/spacen/n_ut,n_pt,n_rt,n_uva,n_uvd,n_up,n_ura,n_urd,

&

n_wp,n_vq,n_pq,n_qr,n_vw,n_vv

common/space2/al,br
common/space3/ai(6,6)
common/spacep/x_uu,x_u

dimension vv(6),beta(3),f(6),rr(3),t(3),c(15,6),ssum(6)
dimension d(6),fi2(6),fd1(6),fs(6),fp(6),ft(6)

setting initial conditions

do j = 1,6
ssum(j) = 0.
d(j) = 0.

fi2(j) = 0.
fd1(j) = 0.
fs(j) = 0.
fp(j) = 0.
ft(j) = 0.

enddo

u = vv(1)
v = vv(2)
w = vv(3)
p = vv(4)
q = vv(5)
r = vv(6)
s1 = sin(beta(1))
s2 = sin(beta(2))
s3 = sin(beta(3))
c1 = cos(beta(1))
c2 = cos(beta(2))
c3 = cos(beta(3))
t2 = s2/c2

u2 = u**2
v2 = v**2
w2 = w**2
p2 = p**2
q2 = q**2
r2 = r**2
uv = u*v

uw = u*w
up = u*p
uq = u*q
ur = u*r
vw = v*w
vp = v*p
vq = v*q
vr = v*r
wp = w*p
wq = w*q
wr = w*r
pq = p*q
pr = p*r
qp = pq
qr = q*r
rp = pr
rq = r*q

c-----------------------------------------------------------c

CALULATING VARIOUS FORCE ELEMENTS

c------------------------------------------------------------

c-----------------------------------------------------------c

calculate FORCE-VECTOR {d(i)}

d(1) = m*(vr - wq + xg*(q2 + r2) - yg*pq - zg*pr)

d(2) = m*(wp - ur + yg*(r2 + p2) - zg*qr - xg*qp)
d(3) = m*(uq - vp + zg*(p2 + q2) - xg*rp - yg*rq)
d(4) = (iyy-izz)*qr + ixz*pq + iyz*(q2-r2) - ixy*pr

&

- m*yg*(vp-uq) + m*zg*(ur-wp)
d(5) = (izz-ixx)*rp + iyx*qr + izx*(r2-p2) - iyz*qp

&

- m*zg*(wq-vr) + m*xg*(vp-uq)
d(6) = (ixx-iyy)*pq + izy*rp + ixy*(p2-q2) - izx*rq

&

- m*xg*(ur-wp) + m*yg*(wq-vr)

c------------------------------------------------------------

c-----------------------------------------------------------c

calculate FORCE-VECTOR {fi2(i)}

fi2(1) = x_wq*wq + x_qq*q2 + x_vr*vr + x_rr*r2

&

+ x_rp*rp + x_uq*uq
fi2(2) = y_ura*ur + y_wp*wp + y_pq*pq + y_up*up

&

+ y_wr*wr + y_qr*qr
fi2(3) = z_uqa*uq + z_vp*vp + z_rp*rp + z_pp*p2

&

+ z_wq*wq + z_qq*q2
fi2(4) = k_vw *vw + k_vq*vq + k_wr*wr + k_wp*wp

&

+ k_uva*uv+ k_ura*ur + k_pq*pq + k_qr*qr

fi2(5) = m_uqa*uq + m_uwa*uw + m_vr*vr + m_vp*vp

&

+ m_rr*r2+ m_pp*p2 + m_rp*rp + m_wq*wq

&

+ m_uu*u2+ m_wwa*w2
fi2(6) = n_uva*uv + n_up*up + n_ura*ur+ n_wp*wp

&

+ n_vq*vq + n_pq*pq + n_qr*qr + n_vw*vw

c-------------------------------------------------------------------

c------------------------------------------------------------------c

calculate FORCE-VECTOR fd1(i)

fd1(1) = x_v*abs(v) + x_vv*v2 + x_w*abs(w) + x_ww*w2

fd1(2) = y_uv*uv + y_vv*v*sqrt(v2+w2) + y_urd*ur

fd1(3) = z_uw*uw + z_ww*w*sqrt(v2+w2) + z_uqd*uq + z_st*u2
fd1(4) = k_uvd*uv + k_up*up + k_urd*ur
fd1(5) = m_uwd*uw + m_wwd*w*sqrt(v2+w2) + m_uqd*uq + m_st*u2
fd1(6) = n_uvd*uv + n_vv*v*sqrt(v2+w2) + n_urd*ur
c-------------------------------------------------------------------

c----------------------------------------------------------------------c

calculate FORCE-VECTOR fs(i)

this is the hydrostatic force

here volume can be take as depth dependent

fz = function(z= ro(3)); ro(3) needs to be entered in subroutine

wt = m*grav
b = by*grav

fs(1) = -(wt-b)*s2
fs(2) = (wt-b)*c2*s1
fs(3) = (wt-b)*c2*c1
fs(4) = (yg*wt - yb*b)*c2*c1 - (zg*wt - zb*b)*c2*s1
fs(5) = -(zg*wt - zb*b)*s2 - (xg*wt - xb*b)*c2*c1
fs(6) = (xg*wt - xb*b)*c2*s1 + (yg*wt - yb*b)*s2
c------------------------------------------------------------------------

c-----------------------------------------------------------------------c

calculate FORCE-VECTOR fp(i)

res_t = x_uu*u2 + x_u*u

fp_in = 0.
fp(1) = fp_in
fp(1) = fp(1) - res_t

write(10,1001) time,uc,fp_in,u,res_t,fp(1)

1001

format(6f12.4)

c------------------------------------------------------------------------------

c*****************************************************************************
c

HERE, ALL OTHER COMPONENTS OF THE FORCE VECTOR ARE TO BE FOUND

EITHER BY CALLING A SUBROUTINE, OR DIRECTLY

FOR EXAMPLE, BY CALLING, .. subroutine force_k(**,**,force_k(j))

THEN ADDING THESE FORCES TOTHE TOTAL

c*****************************************************************************

c----------------------------------------------------------------------------c

all forcees are over

c-----------------------------------------------------------------------------

c----------------------------------------------------------------------------c

summing of forces to get total

do j = 1,6
ft(j)=d(j)+fi2(j)+fd1(j)+fs(j)+fp(j)
enddo
c-----------------------------------------------------------------------------

write(10,2222) time,(ft(j),j=1,6)

2222

format(f8.2,6f16.6)

carrying to operation [a]^-1 * {ft}

do i = 1,6
f(i) = 0.
do j = 1,6
f(i) = f(i) + ai(i,j) * ft(j)
enddo
enddo
c--------------------------------------------------------------------------

c-------------------------------------------------------------------------c

calculate {rr(i)} and {t(i)}

rr(1) = p + s1*t2*q + c1*t2*r

rr(2) =
rr(3) =

c1*q -

s1*r

s1/c2*q + (c1/c2)*r

t(1) = c2*c3*u + (-c1*s3+s1*s2*c3)*v + ( s1*s3+c1*s2*c3)*w

t(2) = c2*s3*u + ( c1*c3+s1*s2*s3)*v + (-s1*c3+c1*s2*s3)*w
t(3)= -s2*u +

(s1*c2)*v +

(c1*c2)*w

c------------------------------------------------------------------------return
end
c-------------------------------------------------------------------------

subroutine to calculate area

subroutine area(n,dx,y,ar)
dimension y(25)
ar = 0.
do 1 i = 2,n
1

ar = ar + 0.5 * dx * ( y(i-1) + y(i) )

return
end

subroutine to invert matrix

subroutine invert(np,a,y)
dimension a(np,np),y(np,np),indx(6)
n = np
do 12 i = 1,n
do 11 j = 1,n
y(i,j) = 0.
11

continue
y(i,i) = 1.

12 continue
call ludcmp(a,n,np,indx,d)
do 13 j = 1,n
call lubksb(a,n,np,indx,y(1,j))
13

continue

return
end

subroutine for lu decomposition

subroutine ludcmp(a,n,np,indx,d)
parameter (nmax=10,tiny=1.0e-20)
dimension a(np,np),indx(6),vv(nmax)

d=1
do 12 i = 1,n
aamax = 0.
do 11 j = 1,n
if (abs(a(i,j)) .gt. aamax) aamax = abs(a(i,j))
11 continue
c

if (aamax .eq. 0.) pause 'singular matrix'

vv(i) = 1./aamax
12 continue
do 19 j = 1,n
do 14 i = 1,j-1
sum = a(i,j)
do 13 k = 1,i-1
sum = sum - a(i,k) * a(k,j)
13

continue
a(i,j) = sum

14 continue
aamax = 0.
do 16 i = j,n
sum = a(i,j)
do 15 k = 1,j-1
sum = sum - a(i,k) * a(k,j)
15

continue
a(i,j) = sum
dum = vv(i) * abs(sum)
if ( dum .ge. aamax ) then
imax = i
aamax = dum
endif

16

continue
if ( j .ne. imax ) then
do 17 k = 1,n
dum = a(imax,k)
a(imax,k) = a(j,k)
a(j,k) = dum

17

continue
d = -d
vv(imax) = vv(j)
endif
indx(j) = imax
if (a(j,j) .eq. 0.) a(j,j) = tiny

if ( j .ne. n ) then
dum = 1./a(j,j)
do 18 i = j+1,n
a(i,j) = a(i,j) * dum
18 continue
endif
19 continue

return
end

solve the system ax = b, where a = lu decomposition of orig. a

subroutine lubksb(a,n,np,indx,b)
dimension a(np,np),indx(6),b(np)
ii = 0
do 12 i = 1,n

ll = indx(i)
sum = b(ll)
b(ll) = b(i)
if ( ii .ne. 0 ) then
do 11 j = ii,i-1
sum = sum - a(i,j) * b(j)
11

continue
else if ( sum .ne. 0. ) then
ii = i
endif
b(i) = sum

12 continue
do 14 i = n,1,-1
sum = b(i)
do 13 j = i+1,n
sum = sum - a(i,j) * b(j)
13 continue
b(i) = sum/a(i,i)
14 continue

return
end