; FLYDE3D (float three-dimensional) 

; 

; -iteratively and graphically determines the neutral depth and 

;  orientation of an immersed body form 

; -differs from FLYDE.PRO in that it produces true 3D mesh output

; -January 2012: added facility for adding extra components to models using

;  the component file used for 2D drawings

; -February 2012: added facility for drawing 2D, black&white lateral silhouettes

;  with superimposed centres of mass and buoyancy symbols via flag 'SILHO'

;

; SUBROUTINE SOURCES:

;		flyde3d_tools.pro:

;  			init_limb, make_water_box, make_water_surf, set_colours do_shading, 

;			set_wet_dry, set_aspect, draw_silho

;		flyde_tools.pro:

;			limb_data, shift_bod, locate_rot_pt, rot_bod, rot_displ_limb,

;			limb_immersion, body_immersion, set_plot_flyde, draw_body_lat,

;			draw_body_dor, draw_limb_lat, trunk_pressure, minpoint

;

; INPUT: 

; taxon -a string with either a pathname to a data directory or a DMH 

;        shorthand for a model

;

; KEYWORDS:

;

; FRAMES -optional value for the number of frames of animation to generate

;        -default is 32 frames

; STILLS -number of frames to capture for PostScript output

; WRITE -write the animation data to disk

; DEPTH -optional initial depth value to immmerse the model at

; DIP   -optional initial inclination to set the model at

; NOGRAB -don't capture frames of animation

; STONES -three element array with the mass and X- and Y-displacements 

;         relative to the acetabulum of a set of gastroliths 

; UNI -set the density to all one value (passed in UNI)

; DENSCALE -scaling factor to apply body density

; LDEFL -lung 'deflation' factor - increases the density of the lung

;        slices to simulate decreasing lung cavity space

; TEXT_STRING -a string to be used to label a plot or contain a name for

;              an animation file 

; TEXT_POS -a two element array with the X and Y coords for TEXT_STRING

;          -text will be left-justified

; FLEX   -three-element flag to flex the body in the sagittal plane

;        -FLEX(0)= angle (in degrees) to flex the body (-ve ventroflex, 

;         +ve dorsoflex)

;        -FLEX(1), FLEX(2) -the starting and ending slice indices of the 

;         region of the body to be flexed

; PS -write PostScript output of the sequence of stabilization

;    -name of output file is in PS

; FSET -2 element parameter used to control the number of plots with PS output

;		-FSET(0): starting frame offset

;      -FSET(1): frame increment value

; FINAL -draw the final equilibrated state as a PostScript file

;       -flag holds the file name string sans extension of '.eps'

; SQUARE -draw a square on the plot to check the aspect ratio

; PAUSE -flag to cause a wait at the end of each equilibrium adjustment

; ARMRAISE -elevate the arms from their standard orientation via rotation

;           about their proximal end about a horizontal axis

; LEGRAISE -elevate the legs from their standard orientation via rotation

;           about their proximal end about a horizontal axis

; HALT -stop when a specified frame value is reaches

; MAX_DEPTH -depth limit that the model cannot go below

; AREA -compute and report the absolute and relative magnitudes of the exposed

;       ('dry') portion of the body

; NOLABEL -don't draw a frame number on the plot

; DAWDLE -wait at the end of the animation loop (couldn't use the word 'WAIT' as

;         it is reserved)

; RHO_WATER -specify the density of the fluid the models are floating in 

;           -default is seawater (1,026 kg/m3) unless the model is an alligator

;            or an ornithischian dinosaur where it is automatically set to 1000

; MODEL -contains a '/'-terminated string with a subdirectory to read from 

;        Body, Lung, Pectoris and Pelvis to find alternate body and lung forms

;        and different limb socket positions

; PROTRACT -an angular measure in degrees by which to protract (+ve) or 

;           retract (-ve) the forelimbs

; SILHO -produce an outline image showing the body, limbs, and any other body

;        components and the CM and CB in lateral view 

; NO_EDGE -flag to turn off drawing polygon edges

; VISCMB - VISual Center of Mass and Buoyancy on FINAL view

;			-mark the CM with a black cross visible on lateral and dorsal views

;			-mark the CB with a white diamond (tilted cube) on the lateral view

;

; OUTPUT: mdata -a 3D block of animation data in a structure of the form

;                returned by 'GET_MOVIE'

;

PRO flyde3d, taxon, mdata, FRAMES=FRAMES, DEPTH=DEPTH, DIP=DIP, PS=PS, $

			FSET=FSET, UNI=UNI, NOGRAB=NOGRAB, STONES=STONES, STILLS=STILLS, $

			WRITE=WRITE, DENSCALE=DENSCALE, LDEFL=LDEFL, PAUSE=PAUSE, $

			TEXT_STRING=TEXT_STRING, TEXT_POS=TEXT_POS, FLEX=FLEX, FINAL=FINAL, $

			SQUARE=SQUARE, ARMRAISE=ARMRAISE, LEGRAISE=LEGRAISE, HALT=HALT, $

			MAX_DEPTH=MAX_DEPTH, AREA=AREA, SILHO=SILHO, NO_EDGE=NO_EDGE, $

			NOLABEL=NOLABEL, DAWDLE=DAWDLE, RHO_WATER=RHO_WATER, MODEL=MODEL, $

			PROTRACT=PROTRACT, VISCMB=VISCMB



common GEOMETRY, dep2, wid2, den2, a, b, N, cm, cm0, $

						dx, slab_vol, imvol, cb, cb0, level

common SHADES0, dorsal_wet, dorsal_dry, ventral_wet, ventral_dry, $

					edge_color, light

common COMPONENTS1, components, n_compo, compo_list, compo_centroids, $

							compo_masses, compo_volumes



if NOT param_present(FRAMES) then $

		FRAMES = 32

N = 100

G = 9.81

WINX = 600		;450

WINY = 340      ;255

BODY = 0

LEG = 1

ARM = 2

if NOT keyword_set(MAX_DEPTH) then $

		MAX_DEPTH = -99

TRUE = 1

FALSE = 0



dep2 = fltarr(4,N+1)	;depths of orthogonally resampled body mesh

wid2=fltarr(4,N+1)		;widths of orthogonally resampled body mesh

den2 = fltarr(2,N+1)	;densities of orthogonally resampled body mesh

den2(0,*) = indgen(N+1)

imvol = fltarr(N)		;volumes of immersed fraction of a slabs

cb = fltarr(3,N)		;centers of bouyancy of immersed slabs

cm = fltarr(3,N)

level=intarr(N)			;immersion state data

z_immersed = fltarr(1,N+1)

z_emmersed = fltarr(1,N+1)

fb_body = fltarr(N)	;buoyant forces for a set of body slabs

vshift = fltarr(FRAMES)		;vertical shifts applied at each iter.

angle = fltarr(FRAMES)		;angular displacements applied at each iter.



if strpos( taxon,'Data' ) eq -1 then begin

		fgets, 'Data/', 'taxa', taxa_table, /Quiet

		rootname = associate( taxa_table, taxon, /PATH )

		name = associate(taxa_table, taxon, /GENUS )

end else begin

		rootname = taxon

		fgets, taxon, 'name', fullname, /Quiet

		name = fullname(0)

end

if strpos(rootname,'Alligator') eq -1 OR $

		strpos(rootname, 'Marginocephalia') eq -1 OR $

		strpos(rootname,'Ornithopoda') then $

		alligator = 0 $

else begin

		alligator = 1

		RHO_WATER = 1000.

end

if NOT keyword_set(RHO_WATER) then $

		RHO_WATER = 1026.



body_dir = 'Body/'

lung_dir = 'Lung/'

leg_socket_dir = 'Pelvis/'

arm_socket_dir = 'Pectoris/'

if keyword_set(MODEL) then begin

		body_dir = body_dir + MODEL

		lung_dir = lung_dir + MODEL

		leg_socket_dir = leg_socket_dir + MODEL

		arm_socket_dir = arm_socket_dir + MODEL

end 



get, rootname, body_dir + 'depth', dep, dslices, /Quiet

get, rootname, body_dir + 'width', wid, /Quiet

get, rootname, leg_socket_dir + 'acetabulum', acet, /Quiet

find_interval, acet(0), dep(0,*), a_int

get, rootname, arm_socket_dir + 'glenoid', glen, /Quiet

find_interval, glen(0), dep(0,*), g_int



fgets, rootname, 'components', components, n_compo, /Quiet

compo_list = list()

compo_centroids = list()

compo_masses = list()

compo_volumes = list()

get_components, rootname



if keyword_set(FLEX) then begin

		; -locate the initial center points of the slabs that bound the 

		;	acetabulum and glenoid by computing the midpoint of the line segment

		;  that joins the dorsal end of the posterior bounding slice to the 

		;  ventral end of the anterior-bounding slice

		;

		acet_mp0 = (dep([2,3],a_int(0)) + dep([0,1],a_int(1)))/2.

		glen_mp0 = (dep([2,3],g_int(0)) + dep([0,1],g_int(1)))/2.



		; -adjust the body shape using the three parameters passed via FLEX

		; -if there is are any added components then we have to drag them along

		;

		if components(0) ne 'empty' then $

			for c = 0, n_compo-1 do begin

				dep_tmp = dep

				wid_tmp = wid 

				component = compo_list(c)

				contour = component.vertices([0,1],*)

				flex2d,dep_tmp,wid_tmp,[0,0],FLEX(0)*!DTOR,FLEX(1),FLEX(2),contour

				component.vertices([0,1],*) = contour

				compo_list(c) = component 

			end

		flex2d, dep, wid, [0,0], FLEX(0)*!DTOR, FLEX(1), FLEX(2)



		; -locate the new center points of the slabs bounding the acetabulum

		;  and the glenoid, determine the (X,Y) displacement, and adjust the 

		;  two limb socket positions

		;

		acet_mp1 = (dep([2,3],a_int(0)) + dep([0,1],a_int(1)))/2.

		glen_mp1 = (dep([2,3],g_int(0)) + dep([0,1],g_int(1)))/2.

		acet(0:1) = acet(0:1) + acet_mp1 - acet_mp0

		glen(0:1) = glen(0:1) + glen_mp1 - glen_mp0

end

trunk_depth = max( norm( dep([0,1],*) - dep([2,3],*) ) )

trunk_length = glen(0) - acet(0)

dorsal_ymid = trunk_depth * 1.5

max_x = distance( dep([0,1],0), dep([2,3],dslices-1) )

get, rootname, body_dir + 'density', den, /Quiet

get, rootname, body_dir + 'roti_z', roti_z, /Quiet



; -get and prepare right and left version of the limbs, their volumes, 

;  masses, and centres of mass

;

init_limb, rootname, 'Arm/', glen, arm_vol, arm_rho, arm_right_cm, rarm, $

				arm_rot_0, PROTRACT=PROTRACT

mooah, rarm, larm

orot_solid, larm, 2, glen(0), glen(1), -.5

arm_left_cm = arm_right_cm

arm_left_cm(2) = -arm_left_cm(2)



init_limb, rootname, 'Leg/', acet, leg_vol, leg_rho, leg_right_cm, $

					rleg, leg_rot_0

mooah, rleg, lleg

orot_solid, lleg, 2, acet(0), acet(1), .3		;offset left limb for vis effect

leg_left_cm = leg_right_cm

leg_left_cm(2) = -leg_left_cm(2)



if keyword_set(UNI) then begin

		arm_mass = arm_vol * UNI

		leg_mass = leg_vol * UNI

end else begin

		arm_mass = arm_vol * arm_rho

		leg_mass = leg_vol * leg_rho

end



; -compute the CM of the whole body (cm0) after adjusting the axial density 

;  to account for the presence of the lungs

;

if NOT param_present(UNI) then begin

		if keyword_set(DENSCALE) then $

			den(1,*) = den(1,*) * DENSCALE

		print, 'Computing volumes of body and lung...'

		cmvlcm, NULL, bvol, blen, bcm, DD=dep, WW=wid, DEN=den, /Quiet, /Vector

		bvol0 = bvol

		bvol00 = sum(bvol)

		get, rootname, lung_dir + 'depth', ldep, /Quiet

		get, rootname, lung_dir + 'width', lwid, /Quiet

		get, rootname, lung_dir + 'density', lden, /Quiet

		cmvlcm, NULL, lvol, llen, lcm, DD=ldep, WW=lwid, DEN=lden, /Quiet, /Vector



		; -if lung deflation is specified then reduce the lung volume by the 

		;  amount contained in LDEFL, and reduce the trunk volume by same amount

		;

		if keyword_set(LDEFL) then begin

			dlvol = sum( LDEFL*lvol )

			lvol = lvol - LDEFL*lvol

			

			; -identify the limits for indices of body slices that lie between the 

			;  shoulders and hips

			;

			istart = a_int(1) + 1

			istop = g_int(0)

;		gwin, 'testing contraction', /Wide

;		symp,dep, /sl

;		osymp,wid,6,/re, /sl

			dv = dep([0,1],istart:istop) - dep([2,3],istart:istop) 

			k = 0.99

			dep([0,1],istart:istop) = dep([2,3],istart:istop) + k*dv

			wid(1,istart:istop) = wid(1,istart:istop) * k

;		osymp, dep([0, 1], istart:istop), 2

;		osymp, wid(*, istart:istop), 2, /re

			cmvlcm, NULL, bvol0, blen, bcm, DD=dep, WW=wid, DEN=den, /Quiet, /Vector

			diff = (bvol00 - sum(bvol0))/dlvol

			i = 0

			while diff lt 0.95 do begin

				dv = dep([0,1],istart:istop) - dep([2,3],istart:istop) 

				dep([0,1],istart:istop) = dep([2,3],istart:istop) + k*dv

				wid(1,istart:istop) = wid(1,istart:istop) * k

;			osymp,dep([0,1],istart:istop),2

;			osymp,wid(*,istart:istop),2

				cmvlcm, NULL, bvol0, blen, bcm, DD=dep, WW=wid, DEN=den, /Quiet, /Vector

				diff = (bvol00 - sum(bvol0))/dlvol

				i = i + 1

			end

			print, 'Effective body volume reduction (% delta lung volume):',diff

		end



		; -adjust the density of the trunk in the neighbourhood of the lungs 

		;  to acount for the presence of the lung cavity

		;

		get, rootname, lung_dir + 'slices', lung_slices, /Quiet

		lung_slices = lung_slices(0:(size(lvol))(1)-1)

		bvol = bvol0

		bvol(lung_slices) = bvol(lung_slices) - lvol

		mass = bvol*den(1,0:dslices-2)

		den(1,0:dslices-2) = mass / bvol0

		den(1,dslices-1) = den(1,dslices-2)

end else begin

		if UNI ne 0. then $

			den(1,*) = UNI $

		else begin

			print, 'FLYDE: Doh! density == 0 !'

			stop

		end

end

cmvlcm, NULL, bvol, blen, cm0, axial_mass0, DD=dep, WW=wid, DEN=den, /Quiet



; -if a gastrolith was specified then extract the positional information, 

;  and the mass of the stones from the keyword parameter, and compute the 

;  radius of a sphere of granitoid rock of density =2.67gm/cc equal to the 

;  specified mass

;

if keyword_set(STONES) then begin

		stone_pos = [acet(0), acet(1), 0] + [STONES(0), STONES(1), 0]

		stone_mass = STONES(2)

		stone_radius = (3*stone_mass/(4*!PI*2670))^(1./3.)

		stone_contour=ellipse(stone_radius,stone_radius,Centre=stone_pos,/Close)

		stone_volume = 4*!PI*stone_radius^3/3

		stone_force = -G*stone_mass + G*1000.*stone_volume

		if stone_pos(1) ge 0.then $

			suffix ='Stones='+string(STONES(2),Format='(F4.2)')+'kg  @ ' + $

					'('+string(stone_pos(0),Format='(F4.2)')+','+$

					string(stone_pos(1),Format='(F4.2)')+')' $

		else $

			suffix ='Stones='+string(STONES(2),Format='(F4.2)')+'kg  @ ' + $

					'('+string(stone_pos(0),Format='(F4.2)')+','+$

					string(stone_pos(1),Format='(F5.2)')+')' 

end else begin

		stone_pos = 0.

		suffix = '(no stones)'

end



; -if no initial depth value is specified, then lower the sagittal profile 

;  so the first slice is centered at y=0

; -otherwise drag the body and its CM down by the specified amount

;

p0 = (dep([0,1],0) + dep([2,3],0)) / 2.

if NOT param_present(DEPTH) then $

		yshift = -p0(1) $

else $

		yshift = DEPTH

HITBOT = FALSE

min_dep = min(dep([1,3],*))

if min_dep + yshift lt MAX_DEPTH then begin

		yshift = MAX_DEPTH - min_dep

		HITBOT = TRUE

end

p0(1) = p0(1) + yshift

cm0(1) = cm0(1) + yshift

shift_body_p, yshift, dep, acet, glen, leg_right_cm, leg_left_cm, $

					arm_right_cm, arm_left_cm, keyword_set(STONES), stone_pos, $

					stone_contour

vadd_solid, rarm, [0, yshift, 0]

vadd_solid, larm, [0, yshift, 0]

vadd_solid, rleg, [0, yshift, 0]

vadd_solid, lleg, [0, yshift, 0]

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do begin

			component = compo_list(c)

			vadd_solid, component, [0, yshift, 0]

			compo_list(c) = component 

			compo_pos = compo_centroids(c)

			compo_pos(1) = compo_pos(1) + yshift

			compo_centroids(c) = compo_pos

		end



; -if no initial dip angle specified, then determine the dip of the line 

;  joining the vertical midpoints of the first and last slices, in order to 

;  tilt the profile so that the snout is also at y=0

; -rotate axial body with respect to the current centre of mass (cm0)

; -rotate the limb sockets, but don't rotate the limbs so that they stay 

;  hanging vertically - only translate the limbs to have them follow the 

;  displaced sockets

;

if NOT param_present(DIP) then begin

		p1 = (dep([0,1],dslices-1) + dep([2,3],dslices-1)) / 2. 

		dip = -atan( p1(1)-cm0(1), p1(0)-cm0(0))

end else $

		dip = dip*!DTOR

rot_bod, cm0(0:1), dip, dep, wid, keyword_set(STONES), stone_pos, $

				stone_contour, keyword_set(SAIL)

z_axis = [0,0,1]

acet_0 = acet

rotate, acet, cm0, z_axis, dip

d_acet = acet - acet_0

leg_right_cm = leg_right_cm + d_acet

leg_left_cm = leg_left_cm + d_acet

vadd_solid, rleg, d_acet

vadd_solid, lleg, d_acet

glen_0 = glen

rotate, glen, cm0, z_axis, dip

d_glen = glen - glen_0

arm_right_cm = arm_right_cm + d_glen

arm_left_cm = arm_left_cm + d_glen

vadd_solid, rarm, d_glen

vadd_solid, larm, d_glen



; -if there are any components make sure they and their centroids are dipped

;

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do begin

			component = compo_list(c)

			rot_solid, component, cm0, z_axis, dip

			compo_list(c) = component 

			compo_pos = compo_centroids(c)

			rotate, compo_pos, cm0, z_axis, dip

			compo_centroids(c) = compo_pos

		end



; -resample the axial body depths, widths, and density at N+1 unispaced pts

;

print, 'Resampling body meshes...'

mip, dep, dvm

dx = (dvm(0,dslices-1) - dvm(0,0)) / N

new_x = dx * findgen(N+1) + dvm(0,0)

dep2(0,*) = new_x

dep2(2,*) = new_x

dep2(1,*) = interpol( dep(1,*), dep(0,*), new_x )

dep2(3,*) = interpol( dep(3,*), dep(2,*), new_x )

wid2(0,*) = new_x

wid2(1,*) = interpol( wid(1,*), wid(0,*), new_x )

den2(1,*) = interpol( den(1,*), dvm(0,*), new_x )



; -calc. the transverse (a) and vertical (b) radii of body subslabs

; -calculate the individual CMs of body sub-slabs

;

cm(0,*) = dx/2 + new_x(0:N-1)

cm(1,*) = (dep2(3,1:*) + dep2(1,1:*) + dep2(3,0:N-1) + dep2(1,0:N-1))/4.

a = interpol( wid2(1,*), wid2(0,*), cm(0,*) )

b = interpol( dep2(3,*), dep2(2,*), cm(0,*) ) - cm(1,*)

axial_vol = sum( !PI*a*b*dx )

axial_mass = sum( !PI*a*b*dx*den2(1,0:N-1) )



; -calculate the total volume of axial and appendicular body regions as well

;  as any additional components

;

body_vol = axial_vol + 2*leg_vol + 2*arm_vol

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do $

			body_vol = body_vol + compo_volumes(c)



; -determine the total mass of axial body and limbs

; -adjust the whole body cm (CM0) to include the limb masses

;

body_mass = axial_mass + 2*leg_mass + 2*arm_mass

cm0 = (axial_mass*cm0 + leg_mass*leg_right_cm + leg_mass*leg_left_cm + $

			arm_mass*arm_right_cm + arm_mass*arm_left_cm) / body_mass



; -adjust the whole body cm (CM0) to include any additional components

; -simultaneously update the body mass with the extra component masses

;

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do begin

			c_mass = compo_masses(c)

			cm0 = (body_mass*cm0 + c_mass*compo_centroids(c)) / (body_mass + c_mass)

			body_mass = body_mass + c_mass

		end

 

; -if there are gastroliths then adjust the whole body CM to account for them

;

if keyword_set(STONES) then $

		cm0 = (body_mass*cm0 + stone_mass*stone_pos) / (body_mass + stone_mass)



; -with a final determination of body mass compute the mean density 

;

if keyword_set(STONES) then $

		mean_rho = (body_mass - 1000*stone_volume+ stone_mass)/ body_vol $

else $

		mean_rho =  body_mass / body_vol



; -determine the gravity related forces and torques of the axial body

;

fg_body = -G*axial_mass

fg_legs = -G*2*leg_mass

fg_arms = -G*2*arm_mass

fg0 = fg_body + fg_legs + fg_arms

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do $

			fg0 = fg0 - G*compo_masses(c)



if keyword_set(STONES) then $

		fg0 = fg0 + stone_force

leg_down = 0

arm_down = 0

if HITBOT then begin

		minpoint, dep, leg_contour, arm_contour, min_list, min_idx

		locate_rot_pt, min_idx, min_list, dep, leg_contour, arm_contour, acet, glen, $

							rot_pt, rot_pt_id, min_pt, min_pt_id 

		f_up = -fg0

		tg0 = (cm0(0) - rot_pt(0))*fg0

end else begin

		f_up = 0.0

		tg0 = 0.0

		rot_pt = cm0(0:1)

		rot_pt_id = BODY

end



; -determine the buoyancy related forces and torques of the axial body

;

body_immersion, z_immersed, z_emmersed

b_idx = where(imvol ne 0 )

if b_idx(0) gt -1 then begin

		fb_body(b_idx) = imvol(b_idx) * RHO_WATER*G

		fb0 = sum(fb_body(b_idx))

		mom_arm_body = cm(0,*) - cm0(0)

		tb0 = sum(fb_body(b_idx) * mom_arm_body(b_idx))

end else begin

		fb0 = 0.

		tb0 = 0.

end



; -determine the buoyancy related forces and torques of the arms and legs

; -adjust the whole body centre of buoyancy (CB0) to include the effects of the

;  immersed limbs

;

leg_fb = leg_vol * RHO_WATER * 9.81

leg_right_cb = leg_right_cm

leg_left_cb = [ leg_right_cb(0), leg_right_cb(1), -leg_right_cb(2) ]

arm_fb = arm_vol * RHO_WATER * 9.81

arm_right_cb = arm_right_cm

arm_left_cb = [ arm_right_cb(0), arm_right_cb(1), -arm_right_cb(2) ]

cb0 = (fb0*cb0 + leg_fb*leg_right_cb + leg_fb*leg_left_cb + $

			arm_fb*arm_right_cb + arm_fb*arm_left_cb) / (fb0 + 2*leg_fb + 2*arm_fb)

cb00 = cb0



; -standard case when arms fully immersed

;

fb0 = fb0 + 2*leg_fb + 2*arm_fb

tb0 = tb0 + 2*leg_fb*(leg_right_cb(0) - cm0(0)) + $

				2*arm_fb*(arm_right_cb(0) - cm0(0))



; -special case for when pterosaur arms are elevated

;

;fb0 = fb0 + 2*leg_fb

;tb0 = tb0 + 2*leg_fb*(leg_right_cb(0) - cm0(0))



body_weight = abs( fg0 )

body_torque = roti_z * G/trunk_length

rel_force = (fb0 + fg0) / body_weight

rel_torque = (tb0 + tg0) / body_torque 



print, 'mean rho:', mean_rho

print, 'fb0:', fb0,'   fg0:',fg0

print, 'tb0:', tb0

print, 'rel_force:', rel_force

print, 'rel_torque:', rel_torque

print, 'CM:',cm0

print, 'CB:',cb0

print, 'acet:', acet

print, 'glen:', glen



; -assemble and orient the 3D version of the body, limbs and any components

; -as it is derived from the current values in 'dep' it will be positioned 

;  at the correct depth

; -actual density values do not matter as this model is just for show

;

cylmesh, rootname, body_dir, body_axial, DD=dep, WW=wid, /Unidens, /NoShow

if components(0) ne 'empty' then $

		for c = 0, n_compo-1 do begin

			component = compo_list(c)

			merge_solids, body_axial, component, body_axial, /Verbose

		end

merge_solids, body_axial, rleg, body

merge_solids, body, lleg, body

merge_solids, body, rarm, body

merge_solids, body, larm, body



; -use the total body length to set the dimensions of a 3D shape to represent

;  the water surface that will be combined with the body model

;

get, rootname, body_dir + 'length', length, /Quiet

ranges, body, xr, yr, zr, /Quiet, /Get

xr = [0, length]

xspan = length

zspan = zr(1) - zr(0)

set_aspect, body, aspect

make_water_surf, xr, yr, zr, water_surf

get, rootname, body_dir + 'shift', vis_shift, /Quiet



if keyword_set(PS) AND keyword_set(FSET) AND NOT keyword_set(SILHO) then begin

		set_plot, 'PS'

		device, filename=PS, Xsize=16, Ysize=20, Xoffset=0, $

					Yoffset=0, /Color, Bits_Per_Pixel=8, /Helvetica

		if keyword_set(FSET) then begin

			shift = FSET(0)

			step = FSET(1)

		end else begin

			shift = 0

			step = 2

		end

		csize = 1.25

		c_y_offset = 0.75

		!P.Font = 0

end else begin

		shift = 0

		step = 1

		csize = 4

		c_y_offset = 0.9

end



set_colours

if (NOT keyword_set(SILHO) AND NOT keyword_set(FINAL)) OR $

		(keyword_set(FSET) AND shift mod step eq 0) then begin

		set_wet_dry, body, aspect, new_vertices, vtx_count, new_polygons, $

   		          newpoly_count, new_aspect, new_opacity

		body_visual = { , name: body.name, slices: body.slices, steps: body.steps, $

                vertices: new_vertices, density: body.density, $

                polygons: new_polygons, polycount: newpoly_count, $

                fill: intarr(newpoly_count), edge: intarr(newpoly_count), $

                opacity: new_opacity, $

                x_min: body.x_min, x_max: body.x_max, x_mid: body.x_mid, $

                y_min: body.y_min, y_max: body.y_max, y_mid: body.y_mid, $

                z_min: body.z_min, z_max: body.z_max, z_mid: body.z_mid }

		body_visual.opacity(*) = 2

		do_shading, body_visual, new_aspect, alligator

		merge_solids, body_visual, water_surf, visual 



		; -rotate the side view to produce an anterior view, and slide the 

		;  front view laterally into position

		;

		if NOT keyword_set(PS) then begin

			front_view = visual 

			orot_solid, front_view, 1, front_view.x_mid, front_view.z_mid, -!PI/2

			vadd_solid, front_view, 0.5 * xspan + zspan

			merge_solids,  visual, front_view, visual

		end



		; -rotate basic body shape about its long axis to produce a dorsal view

		; -slide it upwards into position

		;

		ranges, body_visual, x_extrema, y_extrema, z_extrema, /Get, /Quiet

		orot_solid, body_visual, 0, body_visual.y_mid, body_visual.z_mid, !PI/2

;	vadd_solid, body_visual, [0, 1.25*(vis_shift(1) - body_visual.y_mid), 0]

		vadd_solid, body_visual, [0, y_extrema(1) - body_visual.y_min, 0]

		merge_solids, visual, body_visual, visual

		visual.name = 'visual'

		ranges, visual, /Get, /Quiet

		if NOT keyword_set(NO_EDGE) then $

			visual.edge(*) = 75

		if visual.x_max ge visual.y_max then $

			view_pos = [visual.x_mid, 0.15*(visual.x_max-visual.x_min), visual.z_max] $

		else $

			view_pos = [visual.x_mid, 0.15*(visual.y_max-visual.y_min), visual.z_max]

			

		if NOT keyword_set(PS) then begin

			show_solid, visual, VP=view_pos

			xyouts, 0.85, c_y_offset, int_string(0), Charsize=csize, Color=2, $

						Charthick=3, Align=0.5, /Normal

		end else begin

			show_solid, visual, PS=PS+'_'+int_string(0)+'.ps', XX=8, YY=8, /NoClose

			xyouts, 0.9, c_y_offset, int_string(0), Charsize=csize, Color=0, /Normal

			device, /close

		end



		if keyword_set(PAUSE) then $

			stop

		if NOT keyword_set(NoGrab) AND NOT keyword_set(PS) then begin

			print, 'Grabbing frame #'+int_string(0)

			images = bytarr(!D.X_VSIZE, !D.Y_VSIZE, FRAMES)

			images(*,*,0)=bytscl(tvrd(0,0,!d.x_vsize,!d.y_vsize,Order=1), Top=170)

		end

end

f = 1

prev_torque = 0.

if NOT keyword_set(HALT) then $

		halt_frame = -1 $

else $

		halt_frame = HALT



while ( abs(rel_force) gt 0.01 OR abs(rel_torque) gt 0.02 ) AND $

			 NOT (leg_down AND arm_down) AND f lt FRAMES AND f ne halt_frame do begin



		print, 'Frame ',f

		; -set equilibrating rotation of body about a transverse axis through the

		;  body CM be a function of the current bouyancy and the total body torque

		;  about the tip of the tail

		;

		min = min( dep(1,*), min_idx)

		if NOT HITBOT then begin

			rot_pt = cm0(0:1)

			min_pt = dep([0,1],min_idx)

			min_pt_id = min_idx

		end

;	if abs(rel_torque) gt 0.75*abs(prev_torque) then $

			angle = 0.2*rel_torque 	;0.1 for Rhampho  ;0.2 for Pterano

;	else $

;		angle = rel_torque

		new_rot_pt = 0

		mp = min_pt

		prot2d, mp, rot_pt, angle

		if mp(1) lt MAX_DEPTH then begin

			found = 0

			delta = 0.001

			t = 0

			print, t, MAX_DEPTH, mp(1)

			while NOT found do begin

				t = t + 1

				if mp(1) ge MAX_DEPTH-delta AND mp(1) lt MAX_DEPTH+delta then $

					found = 1

				if mp(1) lt MAX_DEPTH - delta then $

					angle = angle/2.

				if mp(1) gt MAX_DEPTH + delta then $

					angle = angle + angle/2.

				mp = min_pt

				prot2d, mp, rot_pt, angle

				print, t, MAX_DEPTH, mp(1)

			end

			new_rot_pt = 1

		end

		rot_bod, rot_pt, angle, dep, wid, keyword_set(STONES), stone_pos, $

					stone_contour, keyword_set(SAIL)

		acet_0 = acet

		rotate, acet, [rot_pt,0], z_axis, angle

		d_acet = acet - acet_0

		leg_right_cm = leg_right_cm + d_acet

		leg_left_cm = leg_left_cm + d_acet

		glen_0 = glen

		rotate, glen, [rot_pt,0], z_axis, angle

		d_glen = glen - glen_0

		arm_right_cm = arm_right_cm + d_glen

		arm_left_cm = arm_left_cm + d_glen

		if new_rot_pt then begin

			rot_pt = mp

			rot_pt_id = min_pt_id

			new_rot_pt = 0

			HITBOT = TRUE

		end



		; -set shift to displace body vertically be proportional to the

		;  trunk depth times the fraction of the force difference relative to the 

		;  body weigth

		;

		if NOT HITBOT OR (HITBOT and rel_force gt 0.) then begin

			min = min( dep(1,*), min_idx)

			min_pt = dep([0,1],min_idx)

			yshift = 0.75*trunk_depth*rel_force

;		yshift = 0.1*trunk_depth*rel_force

			if min_pt(1) + yshift le MAX_DEPTH then $

				yshift = MAX_DEPTH - min_list(1,0)

			shift_body_p, yshift, dep, acet, glen, leg_right_cm, leg_left_cm, $

							arm_right_cm, arm_left_cm, keyword_set(STONES), stone_pos, $

							stone_contour

			min = min( dep(1,*), min_idx)

			min_pt = dep([0,1],min_idx)

			if min_pt(1) le MAX_DEPTH then begin

				locate_rot_pt,min_idx,min_list,dep,leg_contour,arm_contour,acet,glen, $

									rot_pt, rot_pt_id, min_pt, min_pt_id

				HITBOT = TRUE

				stop

			end else $

				HITBOT = FALSE

		end 



		print, 'Resampling body meshes...'

		mip, dep, dvm

		dx = (dvm(0,dslices-1) - dvm(0,0)) / N

		new_x = dx * findgen(N+1) + dvm(0,0)

		dep2(1,*) = interpol( dep(1,*), dep(0,*), new_x )

		dep2(3,*) = interpol( dep(3,*), dep(2,*), new_x )

		dep2(0,*) = new_x

		dep2(2,*) = new_x

		wid2(1,*) = interpol( wid(1,*), wid(0,*), new_x )

		wid2(0,*) = new_x

		den2(1,*) = interpol( den(1,*), dvm(0,*), new_x )



		; -update the position of the individual slab CMs and the full CM due to body

		;  shifts and shifts in the limbs and determine the weight related forces 

		;  and torques

		;

		cm(0,*) = dx/2 + new_x(0:N-1)

		cm(1,*) = (dep2(3,1:*) + dep2(1,1:*) + dep2(3,0:N-1) + dep2(1,0:N-1))/4.

		a = interpol( wid2(1,*), wid2(0,*), cm(0,*) )

		b = interpol( dep2(3,*), dep2(2,*), cm(0,*) ) - cm(1,*)

		vol = !PI*dx*a*b

		axial_mass = vol*den2(1,0:N-1)

		axial_mass0 = sum(axial_mass) 

		body_mass = axial_mass0 + 2*leg_mass + 2*arm_mass

		cm0(0) = sum(axial_mass*cm(0,*))/axial_mass0

		cm0(1) = sum(axial_mass*cm(1,*))/axial_mass0

		cm0 = (axial_mass0*cm0 + leg_mass*leg_right_cm + leg_mass*leg_left_cm + $

					arm_mass*arm_right_cm + arm_mass*arm_left_cm) / body_mass

		if keyword_set(STONES) then $

			cm0 = (body_mass*cm0 + stone_mass*stone_pos) / (body_mass + stone_mass)



		; -determine the gravity related forces and torques

		;

		slab_vol = !PI * a * b * dx

		fg_body = -G*slab_vol*den2(1,*)

		fg0 = sum(fg_body) + fg_legs + fg_arms

		if keyword_set(STONES) then $

			fg0 = fg0 + stone_force

		if HITBOT then begin

			tg0 = (cm0(0) - rot_pt(0))*fg0

			f_up = -fg0

		end else begin

			tg0 = 0.0

			rot_pt = cm0(0:1)

		end



		; -determine the degree of immersion of the limbs and body, and compute 

		;  the buoyancy related forces and torques of the axial body

		;

		cb00 = cb0

		body_immersion, z_immersed, z_emmersed

		b_idx = where(imvol ne 0 )

		if max(b_idx) gt -1 then begin

			fb_body(b_idx) = imvol(b_idx)*RHO_WATER*G

			fb0 = sum(fb_body(b_idx))

			mom_arm_body = cm(0,*) - cm0(0)  ;rot_pt(0)

			tb0 = sum(fb_body(b_idx)*mom_arm_body(b_idx))

		end else begin

			fb0 = 0.

			tb0 = 0.

		end



		; -adjust the axial body centre of buoyancy (CB0) to include the effects

		;  of the immersed portions of the limbs

		;

		leg_right_cb = leg_right_cm

		leg_left_cb = [ leg_right_cb(0), leg_right_cb(1), -leg_right_cb(2) ]

		arm_right_cb = arm_right_cm

		arm_left_cb = [ arm_right_cb(0), arm_right_cb(1), -arm_right_cb(2) ]

		cb0 = (fb0*cb0 + leg_fb*leg_right_cb + leg_fb*leg_left_cb + $

		       arm_fb*arm_right_cb + arm_fb*arm_left_cb) / (fb0 + 2*leg_fb + 2*arm_fb)

		cb00 = cb0

		fb0 = fb0 + 2*leg_fb + 2*arm_fb

		tb0 = tb0 + 2*leg_fb*(leg_right_cb(0) - cm0(0)) + $

	         2*arm_fb*(arm_right_cb(0) - cm0(0))



		; -elevated pterosaur wing case

		;

		;fb0 = fb0 + 2*leg_fb

		;tb0 = tb0 + 2*leg_fb*(leg_right_cb(0) - cm0(0))



		rel_force = (fb0 + fg0) / body_weight

		prev_torque = rel_torque

		rel_torque = (tb0 + tg0) / body_torque 

		dcb0 = cb0 - cb00

		;cb_diamond(0,*) = cb_diamond(0,*) + dcb0(0)

		;cb_diamond(1,*) = cb_diamond(1,*) + dcb0(1)



		; -regenerate the body, have any axial body components rotated and 

		;  translated with the axial body, and append the suitable displaced limbs

		;

		cylmesh, rootname, body_dir, body_axial, DD=dep, WW=wid, /Unidens,/NoShow

		if components(0) ne 'empty' then $

			for c = 0, n_compo-1 do begin

				component = compo_list(c)

				vadd_solid, component, [0, yshift, 0]

				orot_solid, component, 2, rot_pt(0), rot_pt(1), angle

				compo_list(c) = component 

				merge_solids, body_axial, component, body_axial

			end

		vadd_solid, rleg, [0,yshift,0] + d_acet

		vadd_solid, lleg, [0,yshift,0] + d_acet

		vadd_solid, rarm, [0,yshift,0] + d_glen

		vadd_solid, larm, [0,yshift,0] + d_glen

		merge_solids, body_axial, rarm, body

		merge_solids, body, larm, body

		merge_solids, body, rleg, body

		merge_solids, body, lleg, body

			

		if (NOT keyword_set(SILHO) AND NOT keyword_set(FINAL)) OR $

			(keyword_set(FSET) AND shift mod step eq 0) then begin

			set_wet_dry, body, aspect, new_vertices, vtx_count, new_polygons, $

							newpoly_count, new_aspect, new_opacity

			body_visual = { , name:body.name, slices:body.slices, steps:body.steps, $

						vertices: new_vertices, density: body.density, $

						polygons: new_polygons, polycount: newpoly_count, $

						fill: intarr(newpoly_count), edge: intarr(newpoly_count), $

						opacity: new_opacity, $

						x_min: body.x_min, x_max: body.x_max, x_mid: body.x_mid, $

						y_min: body.y_min, y_max: body.y_max, y_mid: body.y_mid, $

						z_min: body.z_min, z_max: body.z_max, z_mid: body.z_mid }

			body_visual.opacity(*) = 2

			do_shading, body_visual, new_aspect, alligator

			merge_solids, body_visual, water_surf, visual 



			; -rotate the side view to produce an anterior view, and slide the 

			;  front view laterally into position

			;

			if NOT keyword_set(PS) then begin

				front_view = visual

				orot_solid, front_view, 1, front_view.x_mid, front_view.z_mid, -!PI/2

				vadd_solid, front_view, 0.5 * xspan + zspan

				merge_solids, visual, front_view, visual

			end



			; -rotate basic body shape about its long axis to produce a dorsal view

			; -slide it upwards into position

			;

			ranges, body_visual, x_extrema, y_extrema, z_extrema, /Get, /Quiet

			orot_solid, body_visual, 0, body_visual.y_mid, body_visual.z_mid, !PI/2

			yshift = 1.5*water_surf.z_max - body_visual.y_mid

			vadd_solid, body_visual, [0, y_extrema(1) - body_visual.y_min, 0]

;		vadd_solid, body_visual, 1.7*[0, vis_shift(1) - body_visual.y_mid, 0]

			merge_solids, visual, body_visual, visual

			if NOT keyword_set(NO_EDGE) then $

				visual.edge(*) = 75

			ranges, visual, /Get, /Quiet

			visual.name = 'visual'



			if NOT keyword_set(PS) then begin

				show_solid, visual, /NoWindow, VP=view_pos

				xyouts, 0.85, c_y_offset, int_string(f), Charsize=csize, Color=2, $

							Charthick=3, /Normal

			end else begin

				show_solid, visual, /NoClose, PS=PS+'_'+int_string(f)+'.ps', XX=8,YY=8

				xyouts, 0.9, c_y_offset, int_string(f), Charsize=csize,Color=0,/Normal

				device, /close

			end



			if NOT keyword_set(NoGrab) AND NOT keyword_set(PS) then begin

				print, 'Grabbing frame #'+int_string(f)

				images(*,*,f)=bytscl(tvrd(0,0,!d.x_vsize,!d.y_vsize,Order=1), Top=170)

			end

		end



		print, 'fb0:',fb0,'   fg0:',fg0

		print, 'tb0:',tb0,'   tg0:',tg0

		print, 'rel_force:', rel_force

		print, 'rel_torque:', rel_torque

		print, 'yshift:',yshift

		print, 'angle:',angle*!RADEG

		if keyword_set(PAUSE) then $

			stop



		f = f + 1

end

if f eq halt_frame OR keyword_set(DAWDLE) then stop



print, 'Stopped at f=',f-1

if f eq FRAMES then begin

		print, 'FLYDE: not enough frames to attain equilibrium'

		stop

end



snout = (dep2([0,1],N) + dep2([2,3],N))/2.

inclin = atan( snout(1) - cm0(1), snout(0) - cm0(0) )

print, 'Inclination: ', !RADEG*inclin, ' (deg.)'



; -trim off any unused frames to save space

;

if f lt FRAMES-1 AND NOT keyword_set(FINAL) AND NOT keyword_set(SILHO) AND $

		NOT keyword_set(PS) AND NOT keyword_set(NoGrab) then $

			images = images(*,*,0:f-1)



; -shift the colors back into the 'visible region'

;

if NOT keyword_set(NOGRAB) AND NOT keyword_set(FINAL) AND $

		NOT keyword_set(SILHO) then begin

		; -duplicate the first and last frames

		;

		images = [ [[images(*,*,0)]], [[images]], [[images(*,*,f-1)]] ]

		mdata = { , name: 'FLYDE3D:'+name, $

					win_x: !D.X_VSIZE, win_y: !D.Y_VSIZE, $

					frame_count: f+2, frames: images }

end



print, 'CM:',cm0

print, 'CB:',cb0

print, 'Relative CM(x):', 100*(cm0(0)-acet(0))/(glen(0)-acet(0))

trunk_pressure, acet, glen, tp

print, 'trunk pressure:', tp, 'Pa'

print, 'mean body density:', mean_rho



; -find the interval of the orthogonally resampled depths that bracket the 

;  longitudinal coordinate of the CM and determine the semi-major and 

;  semi-minor radii of body at the position of the CM

;

find_interval, cm0(0),dep2(0,*),interval

print, 'Apex:', avg(dep2(3,interval))

print, 'Dorsoventral radius:', avg((dep2(3,interval)-dep2(1,interval))/2)

print, 'Mediolateral radius:', avg(wid2(1,interval))



if keyword_set(SILHO) then begin

		if param_present(PS) then $

			draw_silho, rootname, body, dep, acet, leg_rot_0, glen, arm_rot_0, $

							compo_list, n_compo, cm0, cb0, PS $

		else $

			draw_silho, rootname, body, dep, acet, leg_rot_0, glen, arm_rot_0, $

							compo_list, n_compo, cm0, cb0

end



if keyword_set(FINAL) then begin

		set_wet_dry, body, aspect, new_vertices, vtx_count, new_polygons, $

						newpoly_count, new_aspect, new_opacity

		body_visual = { , name:body.name, slices:body.slices, steps:body.steps, $

						vertices: new_vertices, density: body.density, $

						polygons: new_polygons, polycount: newpoly_count, $

						fill: intarr(newpoly_count), edge: intarr(newpoly_count), $

						opacity: new_opacity, $

						x_min: body.x_min, x_max: body.x_max, x_mid: body.x_mid, $

						y_min: body.y_min, y_max: body.y_max, y_mid: body.y_mid, $

						z_min: body.z_min, z_max: body.z_max, z_mid: body.z_mid }

		body_visual.opacity(*) = 2

		do_shading, body_visual, new_aspect, alligator

		if NOT keyword_set(NO_EDGE) then $

			body_visual.edge(*) = 75

		merge_solids, body_visual, water_surf, visual 

		if keyword_set(VISCMB) then begin

			get_solid, 'Data/Shapes3D/','Cross/', cross, /Quiet

			get_solid, 'Data/Shapes3D/','Cube/', cube, /Quiet

			cube.fill(*) = 255

			orot_solid, cube, 2, 0, 0, !PI/4

			get, rootname, 'Params/Plot/cross-radius', c_rad, /Quiet

			scale, cross, c_rad, /All

			scale, cube, c_rad, /All

			vadd_solid, cube, cb0

			vadd_solid, cube, [0,0,body_axial.z_max]

			vadd_solid, cross, cm0

			vadd_solid, cross, [0,0,body_axial.z_max + 2*c_rad]

			merge_solids, visual, cube, visual

			merge_solids, visual, cross, visual

		end

			

;	front_view = visual

;	orot_solid, front_view, 1, front_view.x_mid, front_view.z_mid, -!PI/2

;	vadd_solid, front_view, 0.5 * xspan + 0.6 * zspan

;	merge_solids, visual, front_view, visual



		; -rotate basic body shape about its long axis to produce a dorsal view

		; -slide it upwards into position

		;



		ranges, body_visual, x_extrema, y_extrema, z_extrema, /Get, /Quiet

		if keyword_set(VISCMB) then begin

			vadd_solid, cross, [0, body_visual.y_max-body_visual.y_mid, -cross.z_mid]

			merge_solids, body_visual, cross, body_visual

		end

		orot_solid, body_visual, 0, body_visual.y_mid, body_visual.z_mid, !PI/2

		vadd_solid, body_visual, [0, y_extrema(1) - body_visual.y_min, 0]

		merge_solids, visual, body_visual, visual



		; -make a scale bar and merge it with the model views

		;

		scale3d, scalebar, Taxo=rootname, /ps

		ranges, scalebar, xr, yr, zr, /Quiet

		scale_length = xr(1) - xr(0)

		height = yr(2)

		vadd_solid, scalebar, [0, -(scale_length + height), 0]

		merge_solids, visual, scalebar, visual



		ranges, visual, /Get, /Quiet

		show_solid, visual, BKGD=1, PS=FINAL, XX=12, YY=12

end



if keyword_set(PS) AND NOT keyword_set(FSET) then begin 

		if param_present(TEXT_STRING) then begin

			if param_present(TEXT_POS) then $

				xyouts, TEXT_POS(0), TEXT_POS(1), TEXT_STRING, Align=0.0, $

							Color=AXIS_COLOR, Charsize=2 $

			else $

				xyouts, 0., 1.1*max(dep(3,*)), TEXT_STRING, Align=0.0, $

							Color=AXIS_COLOR, Charsize=2

		end

end



if keyword_set(PS) then begin

		device, /close

		set_plot, 'X'

		if keyword_set(MULTI) then $

			!P.multi = 0

end 

if keyword_set(PAUSE) then $

		stop



if keyword_set(WRITE) then begin

    ifname = '~/Grafix/' + WRITE + '.img'

    print, 'Writing animation data to ' + ifname

    openw, unit, ifname, /get_lun

    writeu, unit, fix(WINX), fix(WINY), f

    writeu, unit, images

    close, /all

end



END



; SHIFT_BODY_P -apply a vertical displacement to model components

;

PRO shift_body_p, yshift, dep, acet, glen, leg_right_cm, leg_left_cm, $

						arm_right_cm, arm_left_cm, stone_flag, stone_pos, stone_contour



dep([1,3],*) = dep([1,3],*) + yshift

acet(1) = acet(1) + yshift

leg_right_cm(1) = leg_right_cm(1) + yshift

leg_left_cm(1) = leg_left_cm(1) + yshift

glen(1) = glen(1) + yshift

arm_right_cm(1) = arm_right_cm(1) + yshift

arm_left_cm(1) = arm_left_cm(1) + yshift

if stone_flag then begin

    stone_pos(1) = stone_pos(1) + yshift

    stone_contour(1,*) = stone_contour(1,*) + yshift

end

END