
/PREP7

!NSCALE, ,all,,,1e-3,1e-3,1e-3
ALLSEL
EPLOT

!ESEL,u,MAT,,1 !bone
!ESEL,u,MAT,,2 !enamal
!ESEL,u,MAT,,4 !dentine
!ESEL,u,MAT,,8 !PDL incisor & molar
!ESEL,u,MAT,,6 !pulp

!ESEL,u,MAT,,11 !molar


!!!!!!!!!!!!!!!!!! material properties

!!!!!!!!!!!!!!!  molar
MP,EX,11,30000        
MP,PRXY,11,0.3

!!!!!!!!!!!!!!!  pulp
MP,EX,6,2.03    
MP,PRXY,6,0.45

!!!!!!!!!!!!!!!  dentine
MP,EX,4,18300    
MP,PRXY,4,0.31

!!!!!!!!!!!!!!!  enamal
MP,EX,2,82500 
MP,PRXY,2,0.33

!!!!!!!!!!!!!!!  bone
MP,EX,1,20000    
MP,PRXY,1,0.3

!!!!!!!!!!!!!! PDL_incisor & molar
MP,EX,8,50
MP,PRXY,8,0.4

!!!!!!!!!!!!!!

ALLSEL
ESEL,s,,,all
NSLE
  NUMMRG, node 
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!


!!!!!!!!!!!!!!!!!! defining local CS for new joint
CSYS

WPCSYS
WPROTA,-15,,
CSWPLA,100,0


ET,100,MPC184  !!! Define MPC general joint element without creating corresponding elements
KEYOPT,100,1,16
KEYOPT,100,4,0  !General joint with both displacement and rotational degrees of freedom activated


!!! Constraints

NSEL,s,node,,21648
NSEL,a,node,,23195
CM,Node_Selected,NODE

CSYS,100 !!!0 
NROTAT, Node_Selected
D,Node_Selected,UX,0 

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
CSYS,100 !!!0

ALLSEL

!!!! right TMJ nodes
NSEL,s,node,,611100
NSEL,a,node,,611094
NSEL,a,node,,611081
CM,Node_Selected_l,NODE

!!!! left TMJ nodes
NSEL,s,node,,610196
NSEL,a,node,,609639
NSEL,a,node,,609624

CM,Node_Selected_r,NODE

!!!!!!!!!!!!!!!!!!!!!!!
ALLSEL
NROTAT, Node_Selected_l
NROTAT, Node_Selected_r

D,Node_Selected_r,UX,0 
D,Node_Selected_r,UZ,0 

D,Node_Selected_l,UX,0 
D,Node_Selected_l,UY,0 
D,Node_Selected_l,UZ,0 

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!



!!!!!!!!!!!!!!!!!!!!!!!!!! Muscle forces  !!!!!!!!!!!!!!!

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ALLSEL
CSYS
WPCSYS

to_skip=1       ! enter number of lines to skip
/INQUIRE,numlines,LINES,CaviaMuscleForcesCorrected,csv !
to_read=numlines-to_skip


*DEL,mytable,,NOPR
*DIM,mytable,TABLE,to_read,9    

*TREAD,mytable,CaviaMuscleForcesCorrected,csv,,to_skip

*DEL,Cav_MF,,NOPR
*DIM,Cav_MF,ARRAY,to_read,10    
!
*vfun,Cav_MF(1,1),copy,mytable(0,0)     ! Shift down and right
*vfun,Cav_MF(1,2),copy,mytable(0,1)     ! Shift down and right
*vfun,Cav_MF(1,3),copy,mytable(0,2)     ! Shift down and right
*vfun,Cav_MF(1,4),copy,mytable(0,3)     ! Shift down and right
*vfun,Cav_MF(1,5),copy,mytable(0,4)     ! Shift down and right
*vfun,Cav_MF(1,6),copy,mytable(0,5)     ! Shift down and right
*vfun,Cav_MF(1,7),copy,mytable(0,6)     ! Shift down and right
*vfun,Cav_MF(1,8),copy,mytable(0,7)     ! Shift down and right
*vfun,Cav_MF(1,9),copy,mytable(0,8)     ! Shift down and right
*vfun,Cav_MF(1,10),copy,mytable(0,9)    ! Shift down and right


!!!!!!!!!!!!!!!!!! read node number and muscle force vector components !!!!!!!!!!
to_skip=1       ! enter number of lines to skip
/INQUIRE,numlines,LINES,Cavia_MusF,csv !
to_read=numlines-to_skip


*DEL,mytable1,,NOPR
*DIM,mytable1,TABLE,to_read,3

*TREAD,mytable1,Cavia_MusF,csv,,to_skip

*DEL,NodeNumFV,,NOPR
*DIM,NodeNumFV,ARRAY,to_read,4
!
*vfun,NodeNumFV(1,1),copy,mytable1(0,0)    ! Shift down and right
*vfun,NodeNumFV(1,2),copy,mytable1(0,1)    ! Shift down and right
*vfun,NodeNumFV(1,3),copy,mytable1(0,2)    ! Shift down and right
*vfun,NodeNumFV(1,4),copy,mytable1(0,3)    ! Shift down and right

num_row=to_read
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! applyng muscle forces


!*DO,kk,2,10

*DEL,P4Mirror,,NOPR     !points for mirroring muscle PZM-R and its force
*DIM,P4Mirror,ARRAY,6,3 !6 points, each point has XYZ values
sum_MusNode_x=0
sum_MusNode_y=0
sum_MusNode_z=0

jj=1
NumOFNode=9   !the number of node in each muscle group (medial or lateral side)
kk=3          !the column number corresponding muscle loading scenario 


!!!!!! debug file1
/PREP7
*DO,ii,1,num_row-4
!ii=5

VFX=NodeNumFV(ii,2)/sqrt(NodeNumFV(ii,2)**2+NodeNumFV(ii,3)**2+NodeNumFV(ii,4)**2)
VFY=NodeNumFV(ii,3)/sqrt(NodeNumFV(ii,2)**2+NodeNumFV(ii,3)**2+NodeNumFV(ii,4)**2)
VFZ=NodeNumFV(ii,4)/sqrt(NodeNumFV(ii,2)**2+NodeNumFV(ii,3)**2+NodeNumFV(ii,4)**2)

F,NodeNumFV(ii,1),FX,VFX*Cav_MF(jj,kk)/NumOFNode
F,NodeNumFV(ii,1),FY,VFY*Cav_MF(jj,kk)/NumOFNode
F,NodeNumFV(ii,1),FZ,VFZ*Cav_MF(jj,kk)/NumOFNode

 sum_MusNode_x=sum_MusNode_x+NX(NodeNumFV(ii,1))
 sum_MusNode_y=sum_MusNode_y+NY(NodeNumFV(ii,1))
 sum_MusNode_z=sum_MusNode_z+NZ(NodeNumFV(ii,1))



*IF,NodeNumFV(ii,1),EQ,567468,THEN !SM_R
test_nodenum1=NodeNumFV(ii,1)
test_force1=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

test1=sum_MusNode_z

 P4Mirror(1,1)=sum_MusNode_x/NumOFNode   ! mean of SM_L nodes
 P4Mirror(1,2)=sum_MusNode_y/NumOFNode
 P4Mirror(1,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

NumOFNode=9

*ELSEIF,NodeNumFV(ii,1),EQ,565790 !DM-L
test_nodenum1=NodeNumFV(ii,1)
test_force1=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

 P4Mirror(2,1)=sum_MusNode_x/NumOFNode   ! mean of SM_R nodes
 P4Mirror(2,2)=sum_MusNode_y/NumOFNode
 P4Mirror(2,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

jj=jj+2
NumOFNode=27

*ELSEIF,NodeNumFV(ii,1),EQ,603381  !DM-R
test_nodenum2=NodeNumFV(ii,1)
test_force2=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

 P4Mirror(3,1)=sum_MusNode_x/NumOFNode   ! mean of DM_L nodes
 P4Mirror(3,2)=sum_MusNode_y/NumOFNode
 P4Mirror(3,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

NumOFNode=25

*ELSEIF,NodeNumFV(ii,1),EQ,600760  !AZM-L
test_nodenum3=NodeNumFV(ii,1)
test_force3=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

 P4Mirror(4,1)=sum_MusNode_x/NumOFNode   ! mean of DM_R nodes
 P4Mirror(4,2)=sum_MusNode_y/NumOFNode
 P4Mirror(4,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

jj=jj+2
NumOFNode=26

*ELSEIF,NodeNumFV(ii,1),EQ,600704  !AZM-R
test_nodenum4=NodeNumFV(ii,1)
test_force4=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

 P4Mirror(5,1)=sum_MusNode_x/NumOFNode   ! mean of AZM_L nodes
 P4Mirror(5,2)=sum_MusNode_y/NumOFNode
 P4Mirror(5,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

NumOFNode=31

*ELSEIF,NodeNumFV(ii,1),EQ,592974 !PZM-R
test_nodenum5=NodeNumFV(ii,1)
test_force5=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

 P4Mirror(6,1)=sum_MusNode_x/NumOFNode   ! mean of AZM_R nodes
 P4Mirror(6,2)=sum_MusNode_y/NumOFNode
 P4Mirror(6,3)=sum_MusNode_z/NumOFNode

 sum_MusNode_x=0
 sum_MusNode_y=0
 sum_MusNode_z=0
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

jj=jj+2
NumOFNode=27

*ELSEIF,NodeNumFV(ii,1),EQ,609039 !IP-L
test_nodenum6=NodeNumFV(ii,1)
test_force6=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

jj=jj+2
NumOFNode=18

*ELSEIF,NodeNumFV(ii,1),EQ,599738 !IP-R
test_nodenum7=NodeNumFV(ii,1)
test_force7=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

NumOFNode=18

*ELSEIF,NodeNumFV(ii,1),EQ,598034 !EP-R
test_nodenum8=NodeNumFV(ii,1)
test_force8=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

jj=jj+2
NumOFNode=13

*ELSEIF,NodeNumFV(ii,1),EQ,608568 !EP-L
test_nodenum9=NodeNumFV(ii,1)
test_force9=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

NumOFNode=13

!*ELSEIF,NodeNumFV(ii,1),EQ,608198 !IOZM-R & L 
!test_nodenum10=NodeNumFV(ii,1)
!test_force10=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!jj=jj+2
!NumOFNode=1

!*ELSEIF,NodeNumFV(ii,1),EQ,1184529 !T-L & R
!test_nodenum11=NodeNumFV(ii,1)
!test_force11=Cav_MF(jj,kk)*sqrt(VFX**2+VFY**2+VFZ**2)

!jj=jj+2
!NumOFNode=1


*ELSE


*ENDIF


*ENDDO

!/NERR,,,-1 !switch off error checking

!debug file2
/PREP7
!!!!!!!!!!!!!!!!!!!!! mirror PZM-R  !!!!!!!!!!!!!!!
CSYS

!solve mean coordinates between two pairing muscle bundles
AX= (P4Mirror(1,1)+ P4Mirror(2,1))/2 !point A
AY= (P4Mirror(1,2)+ P4Mirror(2,2))/2
AZ= (P4Mirror(1,3)+ P4Mirror(2,3))/2

BX= (P4Mirror(3,1)+ P4Mirror(4,1))/2 !point B
BY= (P4Mirror(3,2)+ P4Mirror(4,2))/2
BZ= (P4Mirror(3,3)+ P4Mirror(4,3))/2

CX= (P4Mirror(5,1)+ P4Mirror(6,1))/2 !point C
CY= (P4Mirror(5,2)+ P4Mirror(6,2))/2
CZ= (P4Mirror(5,3)+ P4Mirror(6,3))/2


! find two vectors in the mirror plane
AB_x=BX-AX
AB_y=BY-AY
AB_z=BZ-AZ

AC_x=CX-AX
AC_y=CY-AY
AC_z=CZ-AZ

!solve normal vector perpendicular to the 3-point plane
NorVec_x=AB_y*AC_z-AC_y*AB_z
NorVec_y=-(AB_x*AC_z-AC_x*AB_z)
NorVec_z=AB_x*AC_y-AC_x*AB_y

!normalize normal vector
NorVec_ux=NorVec_x/sqrt(NorVec_x**2+NorVec_y**2+NorVec_z**2)
NorVec_uy=NorVec_y/sqrt(NorVec_x**2+NorVec_y**2+NorVec_z**2)
NorVec_uz=NorVec_z/sqrt(NorVec_x**2+NorVec_y**2+NorVec_z**2)



! create local CS represent mirror at point A
N,1E6,AX,AY,AZ

N,1E6+20,NX(1E6)+NorVec_ux,NY(1E6)+NorVec_uy,NZ(1E6)+NorVec_uz
N,1E6+21,BX,BY,BZ

CS,100,0,1E6,1E6+20,1E6+21

!debug file3
/PREP7
CSYS,100
WPCSYS

*DEL,ADDNODENum,,NOPR
*DIM,ADDNODENum,ARRAY,500,1


NSEL,s,loc,x,-1000,0

NSEL,s,,,603557
NSEL,a,,,608567
NSEL,a,,,609166
NSEL,a,,,609194
NSEL,a,,,612359
NSEL,a,,,612865
NSEL,a,,,617412
NSEL,a,,,617447
NSEL,a,,,617903
NSEL,a,,,623127
NSEL,a,,,623912
NSEL,a,,,629470
NSEL,a,,,630046
NSEL,a,,,635798
NSEL,a,,,635824
NSEL,a,,,635826
NSEL,a,,,884034
NSEL,a,,,888048
NSEL,a,,,911055
NSEL,a,,,936098

CM,T_RNodes,NODE

*GET, CountNodeNum, NODE, 0, COUNT

CMDELE,resNode
*DO,ii,1,CountNodeNum

NSEL,s,,,T_RNodes
NSEL,u,,,resNode

*GET,ADDNODENum(ii,1),NODE,0,NUM,MIN

NSEL,s,,,ADDNODENum(ii,1)
NSEL,a,,,resNode
CM,resNode,NODE

*ENDDO

!!! mirror nodes of T-R to the left side of skull
ALLSEL
NNSPACE=0.5

*DO,ii,1,CountNodeNum

NSEL,s,loc,y,NY(ADDNODENum(ii,1))-NNSPACE,NY(ADDNODENum(ii,1))
NSEL,r,loc,z,NZ(ADDNODENum(ii,1))-NNSPACE,NZ(ADDNODENum(ii,1))
*GET, LocMaxX, NODE, 0, MXLOC, X
NSEL,r,loc,x,LocMaxX,LocMaxX

*GET, ADDNODENum(ii+CountNodeNum,1), NODE, 0, NUM, MAX

*ENDDO

NSEL,s,,,ADDNODENum(CountNodeNum+1,1)
*DO,ii,CountNodeNum+1,CountNodeNum+CountNodeNum
NSEL,a,,,ADDNODENum(ii,1)
*ENDDO

CM,T_LNodes,NODE


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
NSEL,s,loc,x,-1000,0

NSEL,s,,,547236
NSEL,a,,,547247
NSEL,a,,,548484
NSEL,a,,,548997
NSEL,a,,,549431
NSEL,a,,,549452
NSEL,a,,,549903
NSEL,a,,,550357
NSEL,a,,,551296
NSEL,a,,,551301
NSEL,a,,,551316
NSEL,a,,,551338
NSEL,a,,,551805
NSEL,a,,,551812
NSEL,a,,,553390
NSEL,a,,,553953
NSEL,a,,,555177
NSEL,a,,,556367
NSEL,a,,,557753
NSEL,a,,,559330

CM,IOZM_RNodes,NODE

*GET, CountNodeNum1, NODE, 0, COUNT

CMDELE,resNode
*DO,ii,2*CountNodeNum+1,2*CountNodeNum+CountNodeNum1

NSEL,s,,,IOZM_RNodes
NSEL,u,,,resNode

*GET,ADDNODENum(ii,1),NODE,0,NUM,MIN

NSEL,s,,,ADDNODENum(ii,1)
NSEL,a,,,resNode
CM,resNode,NODE

*ENDDO

!!! mirror nodes of IOZM_R to the left side of skull
ALLSEL
NNSPACE=0.5

*DO,ii,1,CountNodeNum1

NSEL,s,loc,y,NY(ADDNODENum(2*CountNodeNum+ii,1))-NNSPACE,NY(ADDNODENum(2*CountNodeNum+ii,1))
NSEL,r,loc,z,NZ(ADDNODENum(2*CountNodeNum+ii,1))-NNSPACE,NZ(ADDNODENum(2*CountNodeNum+ii,1))
*GET, LocMaxX, NODE, 0, MXLOC, X
NSEL,r,loc,x,LocMaxX,LocMaxX

*GET, ADDNODENum(ii+2*CountNodeNum+CountNodeNum1,1), NODE, 0, NUM, MAX

*ENDDO

NSEL,s,,,ADDNODENum(2*CountNodeNum+CountNodeNum1+1,1)
*DO,ii,2*CountNodeNum+CountNodeNum1+1,2*CountNodeNum+2*CountNodeNum1
NSEL,a,,,ADDNODENum(ii,1)
*ENDDO

CM,IOZM_LNodes,NODE

NSEL,s,,,T_RNodes
NSEL,a,,,T_LNodes
NSEL,a,,,IOZM_RNodes
NSEL,a,,,IOZM_LNodes

! select nodes that are mirrored from PZM-R nodes

CSYS,0 

NSEL,s,,,600708
NSEL,a,,,603344
NSEL,a,,,603351
NSEL,a,,,604263
NSEL,a,,,605151
NSEL,a,,,606800
NSEL,a,,,606827
NSEL,a,,,606830
NSEL,a,,,608970
NSEL,a,,,608986
NSEL,a,,,610603
NSEL,a,,,611636
CM,PZM_LNodes,NODES

NSEL,s,,,596354
NSEL,a,,,597047
NSEL,a,,,597737
NSEL,a,,,599206
NSEL,a,,,599947
NSEL,a,,,600774
NSEL,a,,,601628
NSEL,a,,,603431
NSEL,a,,,604331
NSEL,a,,,604340
NSEL,a,,,605247
NSEL,a,,,606079
NSEL,a,,,606888
NSEL,a,,,607673
NSEL,a,,,609039
CM,PZM_RNodes,NODES

NSEL,s,,,PZM_LNodes
NSEL,a,,,PZM_RNodes

FDELE,all,all
!!!!!!!!!!!!!!!!!!!

PZMNoderx=0
PZMNodery=0
PZMNoderz=0

*DO,rowcounter,140,154

PZMNoderx=PZMNoderx+NX(NodeNumFV(rowcounter,1))
PZMNodery=PZMNodery+NY(NodeNumFV(rowcounter,1))
PZMNoderz=PZMNoderz+NZ(NodeNumFV(rowcounter,1))

*ENDDO 

PZMNoderx=PZMNoderx/15 
PZMNodery=PZMNodery/15 
PZMNoderz=PZMNoderz/15 

N,1E6+1,PZMNoderx,PZMNodery,PZMNoderz !average point of PZM-R muscle bundle, represent the start point of PZM-R muscle vector force


CSYS,0
! determine the point that could help orient PZM-R force
VFX=NodeNumFV(140,2)/sqrt(NodeNumFV(140,2)**2+NodeNumFV(140,3)**2+NodeNumFV(140,4)**2)
VFY=NodeNumFV(140,3)/sqrt(NodeNumFV(140,2)**2+NodeNumFV(140,3)**2+NodeNumFV(140,4)**2)
VFZ=NodeNumFV(140,4)/sqrt(NodeNumFV(140,2)**2+NodeNumFV(140,3)**2+NodeNumFV(140,4)**2)

PZMNodelx=PZMNoderx+VFX
PZMNodely=PZMNodery+VFY
PZMNodelz=PZMNoderz+VFZ

N,1E6+2,PZMNodelx,PZMNodely,PZMNodelz !point directed from average Node (PZMNoderx,PZMNodery,PZMNoderz) of PZM-R bundle, the vector between them represents PZM-R force vector direction

CSYS,100
N,1E6+3,-NX(1E6+2),NY(1E6+2),NZ(1E6+2) !mirrorred point, represent the end point of PZM-L muscle vector force
N,1E6+4,-NX(1E6+1),NY(1E6+1),NZ(1E6+1) !mirrorred point, represent the start point of PZM-L muscle vector force


CSYS,100

!!!!!!!!!!!!!!!!!!! Applying loads on PZM_L nodes
NSEL,s,,,PZM_LNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes
CM,NodesinSec,NODE
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS

VFX=(NX(1E6+3)-NX(1E6+4))/sqrt((NX(1E6+3)-NX(1E6+4))**2+(NY(1E6+3)-NY(1E6+4))**2+(NZ(1E6+3)-NZ(1E6+4))**2)
VFY=(NY(1E6+3)-NY(1E6+4))/sqrt((NX(1E6+3)-NX(1E6+4))**2+(NY(1E6+3)-NY(1E6+4))**2+(NZ(1E6+3)-NZ(1E6+4))**2)
VFZ=(NZ(1E6+3)-NZ(1E6+4))/sqrt((NX(1E6+3)-NX(1E6+4))**2+(NY(1E6+3)-NY(1E6+4))**2+(NZ(1E6+3)-NZ(1E6+4))**2)

ALLSEL
NSEL,s,,,NodesinSec
F,ALL,FX,VFX*Cav_MF(8,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(8,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(8,kk)/SelNumNode

!!!!!!!!!!!!!!!!!!! Applying loads on PZM_R nodes
NSEL,s,,,PZM_RNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes
!CM,NodesinSec,NODE
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS

VFX=NodeNumFV(128,2)/sqrt(NodeNumFV(128,2)**2+NodeNumFV(128,3)**2+NodeNumFV(128,4)**2)
VFY=NodeNumFV(128,3)/sqrt(NodeNumFV(128,2)**2+NodeNumFV(128,3)**2+NodeNumFV(128,4)**2)
VFZ=NodeNumFV(128,4)/sqrt(NodeNumFV(128,2)**2+NodeNumFV(128,3)**2+NodeNumFV(128,4)**2)

F,ALL,FX,VFX*Cav_MF(7,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(7,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(7,kk)/SelNumNode

!!!!!!!!!!!!!!!!!!! Applying loads on T_R nodes
NSEL,s,,,T_RNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS

VFX=NodeNumFV(220,2)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)
VFY=NodeNumFV(220,3)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)
VFZ=NodeNumFV(220,4)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)

F,ALL,FX,VFX*Cav_MF(15,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(15,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(15,kk)/SelNumNode

!!!!!!!!!!!!!!!!!!! Applying loads on T_L nodes, mirroring T_R force vector

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

!!!!!!!!!!!!!!!!!!!
CSYS,0

TLNoderx=0
TLNodery=0
TLNoderz=0

*DO,rowcounter,1,CountNodeNum

TLNoderx=TLNoderx+NX(ADDNODENum(rowcounter,1))
TLNodery=TLNodery+NY(ADDNODENum(rowcounter,1))
TLNoderz=TLNoderz+NZ(ADDNODENum(rowcounter,1))

*ENDDO 

TLNoderx=TLNoderx/CountNodeNum 
TLNodery=TLNodery/CountNodeNum 
TLNoderz=TLNoderz/CountNodeNum 

N,1E6+5,TLNoderx,TLNodery,TLNoderz !average point of TL-R muscle bundle, represent the start point of TL-R muscle vector force


CSYS,0
! determine the point that could help orient T-R force
VFX=NodeNumFV(220,2)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)
VFY=NodeNumFV(220,3)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)
VFZ=NodeNumFV(220,4)/sqrt(NodeNumFV(220,2)**2+NodeNumFV(220,3)**2+NodeNumFV(220,4)**2)

TLNodelx=TLNoderx+VFX
TLNodely=TLNodery+VFY
TLNodelz=TLNoderz+VFZ

N,1E6+6,TLNodelx,TLNodely,TLNodelz !point directed from average Node (TLNoderx,TLNodery,TLNoderz) of TL-R bundle, the vector between them represents TL-R force vector direction

CSYS,100
N,1E6+7,-NX(1E6+6),NY(1E6+6),NZ(1E6+6) !mirrorred point, represent the end point of TL-L muscle vector force
N,1E6+8,-NX(1E6+5),NY(1E6+5),NZ(1E6+5) !mirrorred point, represent the start point of TL-L muscle vector force

CSYS,100


NSEL,s,,,T_LNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS,0 

VFX=(NX(1E6+7)-NX(1E6+8))/sqrt((NX(1E6+7)-NX(1E6+8))**2+(NY(1E6+7)-NY(1E6+8))**2+(NZ(1E6+7)-NZ(1E6+8))**2)
VFY=(NY(1E6+7)-NY(1E6+8))/sqrt((NX(1E6+7)-NX(1E6+8))**2+(NY(1E6+7)-NY(1E6+8))**2+(NZ(1E6+7)-NZ(1E6+8))**2)
VFZ=(NZ(1E6+7)-NZ(1E6+8))/sqrt((NX(1E6+7)-NX(1E6+8))**2+(NY(1E6+7)-NY(1E6+8))**2+(NZ(1E6+7)-NZ(1E6+8))**2)


F,ALL,FX,VFX*Cav_MF(16,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(16,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(16,kk)/SelNumNode

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!







!!!!!!!!!!!!!!!!!!! Applying loads on IOZM_R nodes
NSEL,s,,,IOZM_RNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS

VFX=NodeNumFV(217,2)/sqrt(NodeNumFV(217,2)**2+NodeNumFV(217,3)**2+NodeNumFV(217,4)**2)
VFY=NodeNumFV(217,3)/sqrt(NodeNumFV(217,2)**2+NodeNumFV(217,3)**2+NodeNumFV(217,4)**2)
VFZ=NodeNumFV(217,4)/sqrt(NodeNumFV(217,2)**2+NodeNumFV(217,3)**2+NodeNumFV(217,4)**2)

F,ALL,FX,VFX*Cav_MF(13,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(13,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(13,kk)/SelNumNode

!!!!!!!!!!!!!!!!!!! Applying loads on IOZM_L nodes
NSEL,s,,,IOZM_LNodes

*GET, SelNumNode, NODE, 0, COUNT ! count the number of selected nodes
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

CSYS

VFX=NodeNumFV(218,2)/sqrt(NodeNumFV(218,2)**2+NodeNumFV(218,3)**2+NodeNumFV(218,4)**2)
VFY=NodeNumFV(218,3)/sqrt(NodeNumFV(218,2)**2+NodeNumFV(218,3)**2+NodeNumFV(218,4)**2)
VFZ=NodeNumFV(218,4)/sqrt(NodeNumFV(218,2)**2+NodeNumFV(218,3)**2+NodeNumFV(218,4)**2)

F,ALL,FX,VFX*Cav_MF(14,kk)/SelNumNode
F,ALL,FY,VFY*Cav_MF(14,kk)/SelNumNode
F,ALL,FZ,VFZ*Cav_MF(14,kk)/SelNumNode


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Solve !!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ALLSEL
CSYS
WPCSYS

/SOLU
ANTYPE,STATIC

SOLVE

ALLSEL

/POST1
SET,1,LAST
PLNSOL, S, EQV
/CONTOUR,,9,0,7/9

/plopts,LEG1,0
/plopts,LEG2,0
/plopts,Date,0
/plopts,TITLE,0

/plopts,FRAMES,off

/triad,off

ALLSEL
CSYS
WPCSYS


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

! delete all forces on nodes
!/PREP7
!FDELE,all,all 



