anchor.diff:                 elf(temp_id1) = elf(temp_id1) + hpx(i)*Fc(j)*Jacobian*gw(igp)
              ENDDO
          ENDDO
  -   ENDDO
  -
  -   DEALLOCATE(gp)
  -   DEALLOCATE(gw)
  -   DEALLOCATE(hhx)
  -   DEALLOCATE(hpx)
  -   DEALLOCATE(GLL_temp)
  -   DEALLOCATE(w_temp)
  -
  -   9999 IF(allo_stat/=0) THEN
  -            IF(ALLOCATED(gp))  DEALLOCATE(gp)
  -            IF(ALLOCATED(gw))  DEALLOCATE(gw)
  -            IF(ALLOCATED(hhx)) DEALLOCATE(hhx)
  -            IF(ALLOCATED(hpx)) DEALLOCATE(hpx)
  -            IF(ALLOCATED(GLL_temp)) DEALLOCATE(GLL_temp)
  -            IF(ALLOCATED(w_temp)) DEALLOCATE(w_temp)
  -        ENDIF
  -
  -   END SUBROUTINE BD_ElementMatrixForce
  -
  -   SUBROUTINE BD_GenerateDynamicElementForce(uuN0,uuN,vvN,aaN,     &
  -                                             Stif0,Mass0,gravity,u,&
  -                                             damp_flag,beta,       &
  -                                             elem_total,node_elem,dof_node,ngp,RHS,Reaction)
  -   !----------------------------------------------------------------------------------------
  -   ! This subroutine computes Global mass matrix and force vector for the beam.
  -   !----------------------------------------------------------------------------------------
  +
  +       if (ErrStat >= AbortErrLev) then
  +          call Cleanup()
  +          return
  +       end if
  +
  +   ENDDO
  +
  +   CALL Cleanup()
  +   RETURN
  +
  +CONTAINS
  +   SUBROUTINE Cleanup()
  +      IF(ALLOCATED(gp       ))  DEALLOCATE(gp       )
  +      IF(ALLOCATED(gw       ))  DEALLOCATE(gw       )
  +      IF(ALLOCATED(hhx      ))  DEALLOCATE(hhx      )
  +      IF(ALLOCATED(hpx      ))  DEALLOCATE(hpx      )
  +      IF(ALLOCATED(GLL_temp ))  DEALLOCATE(GLL_temp )
  +      IF(ALLOCATED(w_temp   ))  DEALLOCATE(w_temp   )
  +      IF(ALLOCATED(temp_Naaa))  DEALLOCATE(temp_Naaa)
  +   END SUBROUTINE Cleanup
  +
  +END SUBROUTINE BD_ElementMatrixForce
  +
  +SUBROUTINE BD_GenerateDynamicElementForce(uuN0,uuN,vvN,aaN,     &
  +                                          Stif0,Mass0,gravity,u,&
  +                                          damp_flag,beta,       &
  +                                          elem_total,node_elem,dof_node,ngp,RHS,&
  +                                          ErrStat,ErrMsg)
  +!----------------------------------------------------------------------------------------
  +! This subroutine computes Global mass matrix and force vector to 
  +! calculate the forces along the beam
  +!----------------------------------------------------------------------------------------
      REAL(ReKi),         INTENT(IN   ):: uuN0(:,:) ! Initial position vector
      REAL(ReKi),         INTENT(IN   ):: uuN(:) ! Displacement of Mass 1: m
      REAL(ReKi),         INTENT(IN   ):: vvN(:) ! Velocity of Mass 1: m/s
  
positive.code:        ENDDO
  
         CALL BD_NodalRelRot(Nuu0,node_elem,dof_node,Nrr0,ErrStat2,ErrMsg2)
         CALL BD_NodalRelRot(Nuuu,node_elem,dof_node,Nrrr,ErrStat2,ErrMsg2)
  
         CALL BD_ElemNodalDisp(vvN,node_elem,dof_node,nelem,Nvvv,ErrStat2,ErrMsg2)
  
         CALL BD_StaticElementMatrixForce(Nuu0,Nuuu,Nrr0,Nrrr,Nvvv,EStif0_GL,EMass0_GL,gravity,DistrLoad_GL,&
                                          ngp,node_elem,dof_node,elf)
  
         CALL BD_AssembleRHS(nelem,dof_elem,node_elem,dof_node,elf,RHS,ErrStat2,ErrMsg2)
  
     ENDDO
  
     DEALLOCATE(Nuu0)
     DEALLOCATE(Nuuu)
     DEALLOCATE(Nrr0)
     DEALLOCATE(Nrrr)
     DEALLOCATE(Nvvv)
     DEALLOCATE(EStif0_GL)
     DEALLOCATE(EMass0_GL)
     DEALLOCATE(DistrLoad_GL)
     DEALLOCATE(elf)
  
     9999 IF(allo_stat/=0) THEN
              IF(ALLOCATED(Nuu0)) DEALLOCATE(Nuu0)
              IF(ALLOCATED(Nuuu)) DEALLOCATE(Nuuu)
              IF(ALLOCATED(Nrr0)) DEALLOCATE(Nrr0)
              IF(ALLOCATED(Nrrr)) DEALLOCATE(Nrrr)
              IF(ALLOCATED(Nvvv)) DEALLOCATE(Nvvv)
              IF(ALLOCATED(EStif0_GL)) DEALLOCATE(EStif0_GL)
              IF(ALLOCATED(EMass0_GL)) DEALLOCATE(EMass0_GL)
              IF(ALLOCATED(DistrLoad_GL)) DEALLOCATE(DistrLoad_GL)
              IF(ALLOCATED(elf)) DEALLOCATE(elf)
          ENDIF
  
  
     END SUBROUTINE BD_GenerateStaticElementForce
  
     SUBROUTINE BD_StaticElementMatrixForce(Nuu0,Nuuu,Nrr0,Nrrr,Nvvv,EStif0_GL,EMass0_GL,gravity,DistrLoad_GL,&
                                            ngp,node_elem,dof_node,elf)
  
     !-------------------------------------------------------------------------------
     ! This subroutine total element forces and mass matrices
     !-------------------------------------------------------------------------------
  
     REAL(ReKi),INTENT(IN):: Nuu0(:) ! Nodal initial position for each element
     REAL(ReKi),INTENT(IN):: Nuuu(:) ! Nodal displacement of Mass 1 for each element
     REAL(ReKi),INTENT(IN):: Nrr0(:) ! Nodal rotation parameters for initial position
     REAL(ReKi),INTENT(IN):: Nrrr(:) ! Nodal rotation parameters for displacement of Mass 1
     REAL(ReKi),INTENT(IN):: Nvvv(:) ! Nodal velocity of Mass 1: m/s for each element
     REAL(ReKi),INTENT(IN):: EStif0_GL(:,:,:) ! Nodal material properties for each element
     REAL(ReKi),INTENT(IN):: EMass0_GL(:,:,:) ! Nodal material properties for each element
     REAL(ReKi),INTENT(IN):: gravity(:) !
     REAL(ReKi),INTENT(IN):: DistrLoad_GL(:,:) ! Nodal material properties for each element
     INTEGER(IntKi),INTENT(IN):: ngp ! Number of Gauss points
     INTEGER(IntKi),INTENT(IN):: node_elem ! Node per element
     INTEGER(IntKi),INTENT(IN):: dof_node ! Degrees of freedom per node
  
     REAL(ReKi),INTENT(OUT):: elf(:)  ! Total element force (Fd, Fc, Fb)
  
     REAL(ReKi),ALLOCATABLE:: gp(:) ! Gauss points
     REAL(ReKi),ALLOCATABLE:: gw(:) ! Gauss point weights
     REAL(ReKi),ALLOCATABLE:: hhx(:) ! Shape function
     REAL(ReKi),ALLOCATABLE:: hpx(:) ! Derivative of shape function
     REAL(ReKi),ALLOCATABLE:: GLL_temp(:) ! Temp Gauss-Lobatto-Legendre points
     REAL(ReKi),ALLOCATABLE:: w_temp(:) ! Temp GLL weights
  
     REAL(ReKi):: uu0(6)
     REAL(ReKi):: E10(3)
     REAL(ReKi):: RR0(3,3)
     REAL(ReKi):: kapa(3)
     REAL(ReKi):: E1(3)
     REAL(ReKi):: Stif(6,6)
     REAL(ReKi):: cet
     REAL(ReKi):: uuu(6)
     REAL(ReKi):: uup(3)
     REAL(ReKi):: Jacobian
     REAL(ReKi):: gpr
     REAL(ReKi):: Fc(6)
     REAL(ReKi):: Fd(6)
     REAL(ReKi):: Oe(6,6)
     REAL(ReKi):: Pe(6,6)
     REAL(ReKi):: Qe(6,6)
     REAL(ReKi):: Fg(6)
     REAL(ReKi):: vvv(6)
     REAL(ReKi):: vvp(6)
     REAL(ReKi):: mmm
     REAL(ReKi):: mEta(3)
     REAL(ReKi):: rho(3,3)
     REAL(ReKi):: temp_Naaa(dof_node*node_elem)
     REAL(ReKi):: temp_aaa(6)
  
     INTEGER(IntKi)::igp
     INTEGER(IntKi)::i
     INTEGER(IntKi)::j
     INTEGER(IntKi)::temp_id1
     INTEGER(IntKi)::allo_stat
     INTEGER(IntKi)          :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)    :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER :: RoutineName = 'BD_StaticElementMatrixForce'
  
     ALLOCATE(gp(ngp), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     gp = 0.0D0
  
     ALLOCATE(gw(ngp), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     gw = 0.0D0
  
     ALLOCATE(hhx(node_elem), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     hhx = 0.0D0
  
     ALLOCATE(hpx(node_elem), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     hpx = 0.0D0
  
     ALLOCATE(GLL_temp(node_elem), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     GLL_temp = 0.0D0
  
     ALLOCATE(w_temp(node_elem), STAT = allo_stat)
     IF(allo_stat/=0) GOTO 9999
     w_temp = 0.0D0
  
     elf = 0.0D0
  
  
     CALL BD_GenerateGLL(ngp,GLL_temp,w_temp,ErrStat2,ErrMsg2)
     CALL BD_GaussPointWeight(ngp,gp,gw,ErrStat2,ErrMsg2)
  
     DO igp=1,ngp
         gpr=gp(igp)
         CALL BD_ComputeJacobian(gpr,Nuu0,node_elem,dof_node,gp,GLL_temp,ngp,igp,hhx,hpx,Jacobian,ErrStat2,ErrMsg2)
  
         CALL BD_GaussPointDataAt0(hhx,hpx,Nuu0,Nrr0,node_elem,dof_node,uu0,E10,ErrStat2,ErrMsg2)
         Stif(:,:) = 0.0D0
         Stif(1:6,1:6) = EStif0_GL(1:6,1:6,igp)
         CALL BD_GaussPointData(hhx,hpx,Nuuu,Nrrr,uu0,E10,node_elem,dof_node,&
                                uuu,uup,E1,RR0,kapa,Stif,cet,ErrStat2,ErrMsg2)
         mmm  = 0.0D0
         mEta = 0.0D0
         rho  = 0.0D0
         mmm          = EMass0_GL(1,1,igp)
         mEta(2)      = -EMass0_GL(1,6,igp)
         mEta(3)      =  EMass0_GL(1,5,igp)
         rho(1:3,1:3) = EMass0_GL(4:6,4:6,igp)
  
         CALL BD_GaussPointDataMass(hhx,hpx,Nvvv,temp_Naaa,RR0,node_elem,dof_node,&
                                    vvv,temp_aaa,vvp,mmm,mEta,rho,ErrStat2,ErrMsg2)
         CALL BD_ElasticForce(E1,RR0,kapa,Stif,cet,Fc,Fd,Oe,Pe,Qe,ErrStat2,ErrMsg2)
         CALL BD_GravityForce(mmm,mEta,gravity,Fg,ErrStat2,ErrMsg2)
         Fd(:) = Fd(:) - Fg(:) - DistrLoad_GL(:,igp)
  
         DO i=1,node_elem
             DO j=1,dof_node
  
negative.code:    INTEGER(IntKi),     INTENT(IN   ):: node_total ! Total number of nodes
     INTEGER(IntKi),     INTENT(IN   ):: ngp ! Number of Gauss points
     REAL(ReKi),         INTENT(  OUT):: Force(:)
     INTEGER(IntKi),     INTENT(  OUT):: ErrStat       ! Error status of the operation
     CHARACTER(*),       INTENT(  OUT):: ErrMsg        ! Error message if ErrStat /= ErrID_None
  
     INTEGER(IntKi)                   :: ErrStat2                     ! Temporary Error status
     CHARACTER(ErrMsgLen)             :: ErrMsg2                      ! Temporary Error message
     CHARACTER(*), PARAMETER          :: RoutineName = 'BD_DynamicSolutionForce'
  
     ErrStat = ErrID_None
     ErrMsg  = ""
  
  
     CALL BD_GenerateDynamicElementForce(uuN0,uuN,vvN,aaN,     &
                                         Stif0,Mass0,gravity,u,&
                                         damp_flag,beta,&
                                         elem_total,node_elem,dof_node,ngp,Force,&
                                         ErrStat2,ErrMsg2)
        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  
     if (ErrStat >= AbortErrLev) RETURN
  
  
     END SUBROUTINE BD_DynamicSolutionForce
  
  